From 6fca64edea002a9ce94890cc6bd5adf940c62e52 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 21 Oct 2023 11:35:29 +0800 Subject: [PATCH] update --- components/step_motor_45/step_motor_45.cpp | 73 ++++++++++++++++++++++++++++-- components/step_motor_45/step_motor_45.hpp | 60 ++++++++++++++++++++---- 2 files changed, 120 insertions(+), 13 deletions(-) diff --git a/components/step_motor_45/step_motor_45.cpp b/components/step_motor_45/step_motor_45.cpp index 5f71ad1..17c3a10 100644 --- a/components/step_motor_45/step_motor_45.cpp +++ b/components/step_motor_45/step_motor_45.cpp @@ -33,12 +33,14 @@ stepmotor_control_matrix_t onestepsq2[] = { #define MAX_STEP_TABLE_OFF (ZARRAY_SIZE(onestepsq1) - 1) -void StepMotor45::initialize(StepMotor45Scheduler* scheduler, cfg_t cfg) { +void StepMotor45::initialize(int32_t module_id, StepMotor45Scheduler *scheduler, cfg_t cfg) { m_cfg = cfg; if (m_cfg.zerofilterCount == 0) { m_cfg.zerofilterCount = 20; } + m_module_id = module_id; + // ³õʼ»¯GPIO ZASSERT(cfg.driverPin[0] != PinNull); ZASSERT(cfg.driverPin[1] != PinNull); @@ -109,7 +111,7 @@ void StepMotor45::onTimIRQ1ms() { } } -int32_t StepMotor45::get_zero_pin_state(int32_t& state) { +int32_t StepMotor45::get_zero_pin_state(int32_t &state) { CriticalContext cc; state = getzeropinstate(); // ZLOGI(TAG, "zero_calibration1 %d", getzeropinstate()); @@ -196,7 +198,7 @@ void StepMotor45::schedule() { m_off = 0; } - stepmotor_control_matrix_t* table = NULL; + stepmotor_control_matrix_t *table = NULL; if (m_cfg.mirror) { table = onestepsq2; } else { @@ -242,8 +244,8 @@ int32_t StepMotor45::move_to(int to, int speed) { posmode = true; return (int32_t)0; } -int32_t StepMotor45::move_to(int to) { move_to(to, defaultspeed); } -int32_t StepMotor45::is_reach_target_pos(bool& reach) { +int32_t StepMotor45::move_to(int to) { return move_to(to, defaultspeed); } +int32_t StepMotor45::is_reach_target_pos(bool &reach) { CriticalContext cc; if (posmode) { reach = m_pos == m_targetPos; @@ -281,3 +283,64 @@ void StepMotor45::stop_motor() { setdriverpinstate(1, 1, 1, 1); } + +/******************************************************************************* + * OVERRIDE MODULE * + *******************************************************************************/ + +int32_t StepMotor45::getid(int32_t *id) { + *id = m_module_id; + return (int32_t)0; +} +int32_t StepMotor45::module_stop() { + stop_motor(); + return (int32_t)0; +} +int32_t StepMotor45::module_break() { + stop_motor(); + return (int32_t)0; +} +int32_t StepMotor45::module_get_last_exec_status(int32_t *status) { + *status = 0; + return (int32_t)0; +} +int32_t StepMotor45::module_get_status(int32_t *status) { + if (m_state == kstop) { + *status = 0; + } else { + *status = 1; + } + return (int32_t)0; +} +int32_t StepMotor45::module_get_error(int32_t *iserror) { + *iserror = 0; + return (int32_t)0; +} +int32_t StepMotor45::module_clear_error() { return (int32_t)0; } + +int32_t StepMotor45::module_set_param(int32_t param_id, int32_t param_value) { return err::kmodule_not_find_config_index; } +int32_t StepMotor45::module_get_param(int32_t param_id, int32_t *param_value) { return err::kmodule_not_find_config_index; } + +int32_t StepMotor45::module_readio(int32_t *io) { return (int32_t)0; } +int32_t StepMotor45::module_writeio(int32_t io) { return (int32_t)0; } +int32_t StepMotor45::module_read_adc(int32_t adcindex, int32_t *adc) { return (int32_t)0; } + +/******************************************************************************* + * Motor * + *******************************************************************************/ +int32_t StepMotor45::motor_enable(int32_t enable) { return (int32_t)0; } +int32_t StepMotor45::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return rotate(direction); } +int32_t StepMotor45::motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) { return move_by(distance); } +int32_t StepMotor45::motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) { return move_to(position); } +int32_t StepMotor45::motor_move_to_with_torque(int32_t pos, int32_t torque) { return err::koperation_not_support; } + +int32_t StepMotor45::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return rotate(direction); } +int32_t StepMotor45::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return move_by(distance); } +int32_t StepMotor45::motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return move_to(position); } + +int32_t StepMotor45::motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; } +int32_t StepMotor45::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return zero_calibration(); } +int32_t StepMotor45::motor_read_pos(int32_t *pos) { + *pos = m_pos; + return (int32_t)0; +} diff --git a/components/step_motor_45/step_motor_45.hpp b/components/step_motor_45/step_motor_45.hpp index 090d76a..f64c124 100644 --- a/components/step_motor_45/step_motor_45.hpp +++ b/components/step_motor_45/step_motor_45.hpp @@ -2,6 +2,7 @@ #include "sdk/os/zos.hpp" #include "sdk\components\step_motor_45\step_motor_45_scheduler.hpp" +#include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp" namespace iflytop { using namespace std; @@ -11,7 +12,7 @@ using namespace std; * https://iflytop1.feishu.cn/wiki/MtaawWm8yijb0tkpWoic1IhsnZc */ -class StepMotor45 { +class StepMotor45 : public ZIModule, public ZIMotor { public: typedef enum { kstop, @@ -60,21 +61,24 @@ class StepMotor45 { ZGPIO *m_driverPinZGPIO[4]; ZGPIO *m_zeroPinZGPIO; - cfg_t m_cfg; + cfg_t m_cfg; + int32_t m_module_id; int defaultspeed = 1000; public: - void initialize(StepMotor45Scheduler *scheduler, cfg_t cfg); + void initialize(int32_t module_id, StepMotor45Scheduler *scheduler, cfg_t cfg); - int32_t rotate(int direction) { rotate(direction, defaultspeed); } + int32_t rotate(int direction) { return rotate(direction, defaultspeed); } int32_t rotate(int direction, int speed); int32_t move_by(int pos, int speed); - int32_t move_by(int pos) { move_by(pos, defaultspeed); } + int32_t move_by(int pos) {return move_by(pos, defaultspeed); } int32_t move_to(int to, int speed); int32_t move_to(int to); - int32_t set_default_speed(int speed) { defaultspeed = speed; } + int32_t set_default_speed(int speed) { defaultspeed = speed; + return 0; + } int32_t get_default_speed(int32_t &speed) { speed = defaultspeed; return (int32_t)0; @@ -82,7 +86,9 @@ class StepMotor45 { int32_t is_reach_target_pos(bool &reach); int32_t stop(); int32_t zero_calibration(); - int32_t set_pos(int32_t pos) { m_pos = pos; } + int32_t set_pos(int32_t pos) { m_pos = pos; + + return int32_t(0);} int32_t get_zero_pin_state(int32_t &state); @@ -91,6 +97,44 @@ class StepMotor45 { return (int32_t)0; } + /******************************************************************************* + * OVERRIDE MODULE * + *******************************************************************************/ + + virtual int32_t getid(int32_t *id) override; + virtual int32_t module_stop() override; + virtual int32_t module_break() override; + virtual int32_t module_get_last_exec_status(int32_t *status) override; + virtual int32_t module_get_status(int32_t *status) override; + virtual int32_t module_get_error(int32_t *iserror) override; + virtual int32_t module_clear_error() override; + + virtual int32_t module_set_param(int32_t param_id, int32_t param_value) override; + virtual int32_t module_get_param(int32_t param_id, int32_t *param_value) override; + + virtual int32_t module_readio(int32_t *io) override; + virtual int32_t module_writeio(int32_t io) override; + + virtual int32_t module_read_adc(int32_t adcindex, int32_t *adc) override; + + /******************************************************************************* + * Motor * + *******************************************************************************/ + virtual int32_t motor_enable(int32_t enable) override; + virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override; + virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) override; + virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override; + virtual int32_t motor_move_to_with_torque(int32_t pos, int32_t torque) override; + + virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) override; + + virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + + virtual int32_t motor_read_pos(int32_t *pos); + private: bool getzeropinstate(); void setdriverpinstate(bool s1, bool s2, bool s3, bool s4); @@ -102,4 +146,4 @@ class StepMotor45 { void onTimIRQ1ms(); }; -} // namespace iflytop \ No newline at end of file +} // namespace iflytop