From 1908f8afec32ebb6009e3d43b6306572c8cb9c7e Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 2 Jun 2024 23:36:17 +0800 Subject: [PATCH] init --- .../mini_servo_motor_ctrl_module.cpp | 5 +- .../mini_servo_motor_ctrl_module.hpp | 16 +-- .../step_motor_ctrl_module.hpp | 26 ++-- components/zcancmder/zcan_protocol_parser.cpp | 149 ++++++++++----------- components/zcancmder/zcan_protocol_parser.hpp | 28 ++-- 5 files changed, 107 insertions(+), 117 deletions(-) diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index c51d0d4..0d97fcf 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -123,7 +123,10 @@ int32_t MiniServoCtrlModule::mini_servo_set_mid_point() { if (!suc) return err::ksubdevice_overtime; return 0; } -int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex) {} +int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex, int32_t *val) { + *val = 0; + return 0; +} void MiniServoCtrlModule::befor_motor_move() { creg.m_module_status = 1; diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index fe513a7..880e3dc 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -40,16 +40,16 @@ class MiniServoCtrlModule : public ZIModule, public ZIMiniServo { virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t &val); public: - virtual int32_t mini_servo_enable(int32_t enable); - virtual int32_t mini_servo_read_pos(int32_t *pos); - virtual int32_t mini_servo_active_cfg(); - virtual int32_t mini_servo_stop(int32_t breakstop); + virtual int32_t mini_servo_enable(int32_t enable) override; + virtual int32_t mini_servo_read_pos(int32_t *pos) override; + virtual int32_t mini_servo_active_cfg() override; + virtual int32_t mini_servo_stop(int32_t breakstop) override; - virtual int32_t mini_servo_rotate(int32_t direction); - virtual int32_t mini_servo_move_to(int32_t position); + virtual int32_t mini_servo_rotate(int32_t direction) override; + virtual int32_t mini_servo_move_to(int32_t position) override; - virtual int32_t mini_servo_set_mid_point(); - virtual int32_t mini_servo_read_io_state(int32_t ioindex); + virtual int32_t mini_servo_set_mid_point() override; + virtual int32_t mini_servo_read_io_state(int32_t ioindex, int32_t *val) override; private: void befor_motor_move(); diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index 6850bf5..ddfe6b5 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -64,24 +64,24 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { * ZIStepMotor * ***********************************************************************************************************************/ - virtual int32_t step_motor_enable(int32_t enable); - virtual int32_t step_motor_read_pos(int32_t* pos); - virtual int32_t step_motor_stop(int32_t breakstop); - virtual int32_t step_motor_easy_rotate(int32_t direction); - virtual int32_t step_motor_easy_move_by(int32_t distance); - virtual int32_t step_motor_easy_move_to(int32_t position); - virtual int32_t step_motor_easy_move_to_zero(); - virtual int32_t step_motor_easy_set_current_pos(int32_t pos); - virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction); - virtual int32_t step_motor_active_cfg(); - virtual int32_t step_motor_read_io_state(int32_t ioindex, int32_t* state); + virtual int32_t step_motor_enable(int32_t enable) override; + virtual int32_t step_motor_read_pos(int32_t* pos) override; + virtual int32_t step_motor_stop(int32_t breakstop) override; + virtual int32_t step_motor_easy_rotate(int32_t direction) override; + virtual int32_t step_motor_easy_move_by(int32_t distance) override; + virtual int32_t step_motor_easy_move_to(int32_t position) override; + virtual int32_t step_motor_easy_move_to_zero() override; + virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override; + virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) override; + virtual int32_t step_motor_active_cfg() override; + virtual int32_t step_motor_read_io_state(int32_t ioindex, int32_t* state) override; private: void inter_inverse_kinematics(int32_t motor_pos, int32_t& x); void inter_forward_kinematics(int32_t x, int32_t& motor_pos); - int inter_get_pos(); - int inter_get_pos(int32_t& x); + int inter_get_pos(); + int inter_get_pos(int32_t& x); void befor_motor_move(); void after_motor_move(); diff --git a/components/zcancmder/zcan_protocol_parser.cpp b/components/zcancmder/zcan_protocol_parser.cpp index 379a32c..f1e972c 100644 --- a/components/zcancmder/zcan_protocol_parser.cpp +++ b/components/zcancmder/zcan_protocol_parser.cpp @@ -15,25 +15,11 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { m_cancmder->registerListener(this); REGFN(module_ping); - // REGFN(module_stop); - // REGFN(module_break); - // REGFN(module_get_last_exec_status); REGFN(module_get_status); REGFN(module_set_reg); REGFN(module_get_reg); - // REGFN(module_readio); - // REGFN(module_writeio); - // REGFN(module_read_adc); REGFN(module_get_error); REGFN(module_clear_error); - // REGFN(module_set_inited_flag); - // REGFN(module_get_inited_flag); - // REGFN(module_factory_reset); - // REGFN(module_flush_cfg); - // REGFN(module_active_cfg); - // REGFN(module_read_raw); - // REGFN(module_enable); - // REGFN(module_start); REGFN(step_motor_enable); REGFN(step_motor_read_pos); @@ -43,7 +29,11 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(step_motor_easy_move_to_zero); REGFN(step_motor_easy_set_current_pos); REGFN(step_motor_easy_move_to_io); + REGFN(step_motor_active_cfg); + REGFN(step_motor_stop); + REGFN(step_motor_read_io_state); +#if 0 REGFN(xymotor_enable); REGFN(xymotor_move_by); REGFN(xymotor_move_to); @@ -64,6 +54,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(a8000_optical_read_scanner_adc_val); REGFN(a8000_optical_read_laster_adc_val); REGFN(a8000_optical_scan_current_point_amp_adc_val); +#endif } void ZCanProtocolParser::_registerModule(uint16_t id, ZIModule* module) { m_modulers[id] = module; } void ZCanProtocolParser::registerModule(ZIModule* module) { @@ -137,20 +128,6 @@ int32_t ZCanProtocolParser::module_ping(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(0); return module->module_ping(); } -// int32_t ZCanProtocolParser::module_stop(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(0); -// return module->module_stop(); -// } -// int32_t ZCanProtocolParser::module_break(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(0); -// return module->module_break(); -// } -// int32_t ZCanProtocolParser::module_get_last_exec_status(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(0); -// int32_t* ack = (int32_t*)cxt->ackbuf; -// cxt->acklen = 4; -// return module->module_get_last_exec_status(&ack[0]); -// } int32_t ZCanProtocolParser::module_get_status(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(0); int32_t* ack = (int32_t*)cxt->ackbuf; @@ -167,22 +144,7 @@ int32_t ZCanProtocolParser::module_get_reg(cmdcontxt_t* cxt) { cxt->acklen = 4; return module->module_get_reg(cxt->param[0], &ack[0]); } -// int32_t ZCanProtocolParser::module_readio(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(0); -// int32_t* ack = (int32_t*)cxt->ackbuf; -// cxt->acklen = 4; -// return module->module_readio(&ack[0]); -// } -// int32_t ZCanProtocolParser::module_writeio(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(2); -// return module->module_writeio(cxt->param[0], cxt->param[1]); -// } -// int32_t ZCanProtocolParser::module_read_adc(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(1); -// int32_t* ack = (int32_t*)cxt->ackbuf; -// cxt->acklen = 4; -// return module->module_read_adc(cxt->param[0], &ack[0]); -// } + int32_t ZCanProtocolParser::module_get_error(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(1); int32_t* ack = (int32_t*)cxt->ackbuf; @@ -193,42 +155,7 @@ int32_t ZCanProtocolParser::module_clear_error(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(0); return module->module_clear_error(); } -// int32_t ZCanProtocolParser::module_set_inited_flag(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(1); -// return module->module_set_inited_flag(cxt->param[0]); -// } -// int32_t ZCanProtocolParser::module_get_inited_flag(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(1); -// int32_t* ack = (int32_t*)cxt->ackbuf; -// cxt->acklen = 4; -// return module->module_get_inited_flag(&ack[0]); -// } -// int32_t ZCanProtocolParser::module_factory_reset(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(0); -// return module->module_factory_reset(); -// } -// int32_t ZCanProtocolParser::module_flush_cfg(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(0); -// return module->module_flush_cfg(); -// } -// int32_t ZCanProtocolParser::module_active_cfg(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(0); -// return module->module_active_cfg(); -// } -// int32_t ZCanProtocolParser::module_read_raw(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(1); -// int32_t* ack = (int32_t*)cxt->ackbuf; -// cxt->acklen = ZCANCMD_READ_BUF_MAX_SIZE; -// return module->module_read_raw(cxt->param[0], cxt->ackbuf, &cxt->acklen); -// } -// int32_t ZCanProtocolParser::module_enable(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(1); -// return module->module_enable(cxt->param[0]); -// } -// int32_t ZCanProtocolParser::module_start(cmdcontxt_t* cxt) { -// CHECK_AND_GET_MODULE(0); -// return module->module_start(); -// } + #undef MODULE_CLASS /*********************************************************************************************************************** @@ -276,7 +203,68 @@ int32_t ZCanProtocolParser::step_motor_easy_move_to_io(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(2); return module->step_motor_easy_move_to_io(cxt->param[0], cxt->param[1]); } + +int32_t ZCanProtocolParser::step_motor_active_cfg(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->step_motor_active_cfg(); +} +int32_t ZCanProtocolParser::step_motor_read_io_state(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->step_motor_read_io_state(cxt->param[0], ack); +} + #undef MODULE_CLASS +#define MODULE_CLASS ZIMiniServo +// virtual int32_t mini_servo_enable(int32_t enable) = 0; +// virtual int32_t mini_servo_read_pos(int32_t* pos) = 0; +// virtual int32_t mini_servo_active_cfg() = 0; +// virtual int32_t mini_servo_stop(int32_t breakstop) = 0; + +// virtual int32_t mini_servo_rotate(int32_t direction) = 0; +// virtual int32_t mini_servo_move_to(int32_t position) = 0; + +// virtual int32_t mini_servo_set_mid_point() = 0; +// virtual int32_t mini_servo_read_io_state(int32_t ioindex) = 0; +int32_t ZCanProtocolParser::mini_servo_enable(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->mini_servo_enable(cxt->param[0]); +} +int32_t ZCanProtocolParser::mini_servo_read_pos(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->mini_servo_read_pos(&ack[0]); +} +int32_t ZCanProtocolParser::mini_servo_active_cfg(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->mini_servo_active_cfg(); +} +int32_t ZCanProtocolParser::mini_servo_stop(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->mini_servo_stop(cxt->param[0]); +} +int32_t ZCanProtocolParser::mini_servo_rotate(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->mini_servo_rotate(cxt->param[0]); +} +int32_t ZCanProtocolParser::mini_servo_move_to(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->mini_servo_move_to(cxt->param[0]); +} +int32_t ZCanProtocolParser::mini_servo_set_mid_point(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->mini_servo_set_mid_point(); +} +int32_t ZCanProtocolParser::mini_servo_read_io_state(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + int32_t* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->mini_servo_read_io_state(cxt->param[0], ack); +} + +#if 0 #define MODULE_CLASS ZIXYMotor int32_t ZCanProtocolParser::xymotor_enable(cmdcontxt_t* cxt) { @@ -392,3 +380,4 @@ int32_t ZCanProtocolParser::a8000_optical_scan_current_point_amp_adc_val(cmdcont } #undef MODULE_CLASS +#endif \ No newline at end of file diff --git a/components/zcancmder/zcan_protocol_parser.hpp b/components/zcancmder/zcan_protocol_parser.hpp index c167ad9..055a61b 100644 --- a/components/zcancmder/zcan_protocol_parser.hpp +++ b/components/zcancmder/zcan_protocol_parser.hpp @@ -47,25 +47,11 @@ class ZCanProtocolParser : public IZCanReceiverListener { private: static int32_t module_ping(cmdcontxt_t* cxt); - static int32_t module_stop(cmdcontxt_t* cxt); - static int32_t module_break(cmdcontxt_t* cxt); - // static int32_t module_get_last_exec_status(cmdcontxt_t* cxt); static int32_t module_get_status(cmdcontxt_t* cxt); static int32_t module_set_reg(cmdcontxt_t* cxt); static int32_t module_get_reg(cmdcontxt_t* cxt); - static int32_t module_readio(cmdcontxt_t* cxt); - static int32_t module_writeio(cmdcontxt_t* cxt); - static int32_t module_read_adc(cmdcontxt_t* cxt); static int32_t module_get_error(cmdcontxt_t* cxt); static int32_t module_clear_error(cmdcontxt_t* cxt); - static int32_t module_set_inited_flag(cmdcontxt_t* cxt); - static int32_t module_get_inited_flag(cmdcontxt_t* cxt); - static int32_t module_factory_reset(cmdcontxt_t* cxt); - static int32_t module_flush_cfg(cmdcontxt_t* cxt); - static int32_t module_active_cfg(cmdcontxt_t* cxt); - static int32_t module_read_raw(cmdcontxt_t* cxt); - static int32_t module_enable(cmdcontxt_t* cxt); - static int32_t module_start(cmdcontxt_t* cxt); static int32_t step_motor_enable(cmdcontxt_t* cxt); static int32_t step_motor_stop(cmdcontxt_t* cxt); @@ -76,8 +62,19 @@ class ZCanProtocolParser : public IZCanReceiverListener { static int32_t step_motor_easy_move_to_zero(cmdcontxt_t* cxt); static int32_t step_motor_easy_set_current_pos(cmdcontxt_t* cxt); static int32_t step_motor_easy_move_to_io(cmdcontxt_t* cxt); - + static int32_t step_motor_active_cfg(cmdcontxt_t* cxt); + static int32_t step_motor_read_io_state(cmdcontxt_t* cxt); + static int32_t mini_servo_enable(cmdcontxt_t* cxt); + static int32_t mini_servo_read_pos(cmdcontxt_t* cxt); + static int32_t mini_servo_active_cfg(cmdcontxt_t* cxt); + static int32_t mini_servo_stop(cmdcontxt_t* cxt); + static int32_t mini_servo_rotate(cmdcontxt_t* cxt); + static int32_t mini_servo_move_to(cmdcontxt_t* cxt); + static int32_t mini_servo_set_mid_point(cmdcontxt_t* cxt); + static int32_t mini_servo_read_io_state(cmdcontxt_t* cxt); + +#if 0 static int32_t xymotor_enable(cmdcontxt_t* cxt); static int32_t xymotor_move_by(cmdcontxt_t* cxt); static int32_t xymotor_move_to(cmdcontxt_t* cxt); @@ -98,6 +95,7 @@ class ZCanProtocolParser : public IZCanReceiverListener { static int32_t a8000_optical_read_scanner_adc_val(cmdcontxt_t* cxt); static int32_t a8000_optical_read_laster_adc_val(cmdcontxt_t* cxt); static int32_t a8000_optical_scan_current_point_amp_adc_val(cmdcontxt_t* cxt); +#endif }; } // namespace iflytop \ No newline at end of file