From 68d4e5e6315c6cc16ec1d19cf3f0344716faa6b3 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 13:58:08 +0800 Subject: [PATCH 01/11] update --- .../xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp index b2d90de..0229362 100644 --- a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp @@ -84,12 +84,12 @@ void XYRobotScriptCmderModule::regcmd() { // REG_CMD_WITH_ACK(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack); REG_CMD_WITH_ACK(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack); REG_CMD_WITH_ACK(PREIX, get_base_param, "(id)", 1, I_XYRobotCtrlModule::base_param_t, ack); - REG_CMD_WITH_ACK("xy_robot_ctrl_", read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack); - REG_CMD_WITH_ACK("xy_robot_ctrl_", read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack); - REG_CMD_WITH_ACK("xy_robot_ctrl_", read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack); - REG_CMD___NO_ACK("xy_robot_ctrl_", flush, "(id)", 1); - REG_CMD___NO_ACK("xy_robot_ctrl_", factory_reset, "(id)", 1); - REG_CMD___NO_ACK("xy_robot_ctrl_", move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); }); + REG_CMD_WITH_ACK(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack); + REG_CMD_WITH_ACK(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack); + REG_CMD_WITH_ACK(PREIX, read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack); + REG_CMD___NO_ACK(PREIX, flush, "(id)", 1); + REG_CMD___NO_ACK(PREIX, factory_reset, "(id)", 1); + REG_CMD___NO_ACK(PREIX, move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); }); m_cmdScheduler->registerCmd("xy_robot_ctrl_set_base_param", "(id,paramName,val)", 3, [this](CmdScheduler::Context* con) { const char* paramName = con->getString(2); From fab2a5bfd47b9e7ac392577f700d3f2a099f0b71 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 14:10:39 +0800 Subject: [PATCH 02/11] update --- components/step_motor_45/step_motor_45.cpp | 5 +++-- components/step_motor_45/step_motor_45.hpp | 3 ++- components/step_motor_45/step_motor_45_scheduler.cpp | 1 + components/step_motor_45/step_motor_45_scheduler.hpp | 4 ++-- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/components/step_motor_45/step_motor_45.cpp b/components/step_motor_45/step_motor_45.cpp index 2a683c6..f732a46 100644 --- a/components/step_motor_45/step_motor_45.cpp +++ b/components/step_motor_45/step_motor_45.cpp @@ -33,7 +33,7 @@ stepmotor_control_matrix_t onestepsq2[] = { #define MAX_STEP_TABLE_OFF (ZARRAY_SIZE(onestepsq1) - 1) -void StepMotor45::initialize(cfg_t cfg) { +void StepMotor45::initialize(StepMotor45Scheduler* scheduler, cfg_t cfg) { m_cfg = cfg; // 初始化GPIO @@ -54,6 +54,7 @@ void StepMotor45::initialize(cfg_t cfg) { m_zeroPinZGPIO->initAsInput(cfg.zeroPin, ZGPIO::kMode_pullup, ZGPIO::kIRQ_noIrq, cfg.zeroPinMirror); } setdriverpinstate(1, 1, 1, 1); + scheduler->addMotor(this); return; } @@ -221,7 +222,7 @@ int32_t StepMotor45::move_to(int to, int speed) { CriticalContext cc; if (to == m_pos) { - return (int32_t)0; + return (int32_t)0; } if (to <= m_pos) { diff --git a/components/step_motor_45/step_motor_45.hpp b/components/step_motor_45/step_motor_45.hpp index c04f77d..42ecaa3 100644 --- a/components/step_motor_45/step_motor_45.hpp +++ b/components/step_motor_45/step_motor_45.hpp @@ -1,6 +1,7 @@ #pragma once #include "sdk/os/zos.hpp" +#include "sdk\components\step_motor_45\step_motor_45_scheduler.hpp" namespace iflytop { using namespace std; @@ -59,7 +60,7 @@ class StepMotor45 { int defaultspeed = 1000; public: - void initialize(cfg_t cfg); + void initialize(StepMotor45Scheduler *scheduler, cfg_t cfg); int32_t rotate(int direction) { rotate(direction, defaultspeed); } int32_t rotate(int direction, int speed); diff --git a/components/step_motor_45/step_motor_45_scheduler.cpp b/components/step_motor_45/step_motor_45_scheduler.cpp index a50e707..eb1d6fd 100644 --- a/components/step_motor_45/step_motor_45_scheduler.cpp +++ b/components/step_motor_45/step_motor_45_scheduler.cpp @@ -1,5 +1,6 @@ #include "step_motor_45_scheduler.hpp" +#include "step_motor_45.hpp" using namespace iflytop; void StepMotor45Scheduler::initialize(zchip_tim_t* tim, int freq) { diff --git a/components/step_motor_45/step_motor_45_scheduler.hpp b/components/step_motor_45/step_motor_45_scheduler.hpp index d170e0b..02431a4 100644 --- a/components/step_motor_45/step_motor_45_scheduler.hpp +++ b/components/step_motor_45/step_motor_45_scheduler.hpp @@ -1,10 +1,10 @@ #pragma once #include "sdk/os/zos.hpp" -#include "step_motor_45.hpp" +// #include "step_motor_45.hpp" namespace iflytop { using namespace std; - +class StepMotor45; class StepMotor45Scheduler { public: zchip_tim_t* m_htim; From 37e343e39ad6b4f7334cb56842131a5709f9ac0b Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 14:11:49 +0800 Subject: [PATCH 03/11] update --- components/tmc/ic/ztmc5130.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp index 5a136d3..2148c52 100644 --- a/components/tmc/ic/ztmc5130.cpp +++ b/components/tmc/ic/ztmc5130.cpp @@ -209,8 +209,6 @@ int32_t TMC5130::readInt(uint8_t address) { return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4]; } -#endif - int32_t TMC5130::to_motor_acc(int32_t acc) { // int32_t val = acc / 60.0 * 51200; return val; @@ -231,4 +229,6 @@ int32_t TMC5130::to_user_pos(int32_t pos) { // int32_t TMC5130::to_user_vel(int32_t vel) { // int32_t val = vel * 60.0 / 51200.0; return val; -} \ No newline at end of file +} +#endif + From 970987eca0f3152271a0137e9276b7652c8bec09 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 14:38:48 +0800 Subject: [PATCH 04/11] fix some bug --- components/mini_servo_motor/feite_servo_motor.cpp | 11 +++++++++++ components/mini_servo_motor/feite_servo_motor.hpp | 4 ++-- components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp | 6 +++++- 3 files changed, 18 insertions(+), 3 deletions(-) diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp index 48f07e7..f60b5e4 100644 --- a/components/mini_servo_motor/feite_servo_motor.cpp +++ b/components/mini_servo_motor/feite_servo_motor.cpp @@ -14,11 +14,13 @@ using namespace feite; } static void dumphex(char* tag, uint8_t* data, uint8_t len) { +#if 0 printf("%s:", tag); for (int i = 0; i < len; i++) { printf("%02x ", data[i]); } printf("\n"); +#endif } void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx) { m_uart = uart; @@ -268,6 +270,7 @@ bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbi } bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) { // // ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len); + cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; cmd_header->header = 0xffff; @@ -280,6 +283,9 @@ bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* da int txpacketlen = sizeof(cmd_header_t) + 1 + len + 1; int rxpacketlen = sizeof(receipt_header_t) + 1; + ZASSERT(txpacketlen < sizeof(m_txbuf)); + ZASSERT(rxpacketlen < sizeof(m_rxbuf)); + uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen); m_txbuf[txpacketlen - 1] = checksum; if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) { @@ -307,6 +313,9 @@ bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t l int rxpacketlen = sizeof(receipt_header_t) + 1 + len; uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen); + ZASSERT(txpacketlen > 0); + ZASSERT(txpacketlen < sizeof(m_txbuf)); + ZASSERT(rxpacketlen < sizeof(m_rxbuf)); m_txbuf[txpacketlen - 1] = checksum; if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) { @@ -323,6 +332,7 @@ bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uin uint32_t enter_ticket = HAL_GetTick(); dumphex("tx:", tx, txdatalen); +#if 1 HAL_UART_Transmit(m_uart, tx, txdatalen, 1000); HAL_UART_Receive_DMA(m_uart, (uint8_t*)rx, expectrxsize); @@ -348,6 +358,7 @@ bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uin return false; } return true; +#endif } bool FeiTeServoMotor::readversion(uint8_t id, uint8_t& mainversion, uint8_t& subversion, uint8_t& miniserv_mainversion, uint8_t& miniserv_subversion) { diff --git a/components/mini_servo_motor/feite_servo_motor.hpp b/components/mini_servo_motor/feite_servo_motor.hpp index 512e3fc..5ea01c8 100644 --- a/components/mini_servo_motor/feite_servo_motor.hpp +++ b/components/mini_servo_motor/feite_servo_motor.hpp @@ -122,8 +122,8 @@ class FeiTeServoMotor { DMA_HandleTypeDef* m_hdma_rx; DMA_HandleTypeDef* m_hdma_tx; - uint8_t m_txbuf[128] = {0}; - uint8_t m_rxbuf[128] = {0}; + uint8_t m_txbuf[256] = {0}; + uint8_t m_rxbuf[256] = {0}; public: void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx); 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 4ca2ebc..0816e18 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -10,7 +10,11 @@ static void limit_input(s32& input, s32 min, s32 max) { if (input < min) input = min; } -void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); } +void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { + m_bus = bus; + m_id = id; + m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); +} int32_t MiniRobotCtrlModule::enable(u8 enable) { bool suc = m_bus->write_u8(m_id, feite::reg_add_e::kRegServoTorqueSwitch, enable); From b953c9fc5b6b58c1fcccf72e4bfa9686289cf0df Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 15:40:51 +0800 Subject: [PATCH 05/11] update --- components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp | 1 + 1 file changed, 1 insertion(+) 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 0816e18..1a88a4f 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -14,6 +14,7 @@ void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { m_bus = bus; m_id = id; m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); + m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal); } int32_t MiniRobotCtrlModule::enable(u8 enable) { From 211cd2315e154d07d22944cad255e48a70ae4cfc Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 16:49:47 +0800 Subject: [PATCH 06/11] update --- components/cmdscheduler/cmd_scheduler.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/components/cmdscheduler/cmd_scheduler.cpp b/components/cmdscheduler/cmd_scheduler.cpp index 6021d3f..211f88f 100644 --- a/components/cmdscheduler/cmd_scheduler.cpp +++ b/components/cmdscheduler/cmd_scheduler.cpp @@ -7,7 +7,7 @@ #include "sdk\components\zprotocols\errorcode\errorcode.hpp" using namespace iflytop; -#define TAG "CmdScheduler" +#define TAG "CMD" void CmdScheduler::registerCmd(std::string cmd, const char* helpinfo, int npara, call_cmd_t call_cmd) { CMD cmdinfo; @@ -19,13 +19,17 @@ void CmdScheduler::registerCmd(std::string cmd, const char* helpinfo, int npara, void CmdScheduler::regbasiccmd() { this->registerCmd("help", "", 0, [this](Context* context) { - ZLOGI(TAG, "help"); ZLOGI(TAG, "cmdlist:"); for (auto it = m_cmdMap.begin(); it != m_cmdMap.end(); it++) { ZLOGI(TAG, " %s %s", it->first.c_str(), it->second.help_info.c_str()); } return (int32_t)0; }); + this->registerCmd("sleep_ms", "", 1, [this](Context* context) { + int ms = atoi(context->argv[1]); + osDelay(ms); + return 0; + }); } void CmdScheduler::initialize(UART_HandleTypeDef* huart, uint32_t rxbufsize) { From c81e01b69c3648d3397ac771773cee50357032b7 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 16:50:05 +0800 Subject: [PATCH 07/11] update --- components/cmdscheduler/cmd_scheduler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/cmdscheduler/cmd_scheduler.cpp b/components/cmdscheduler/cmd_scheduler.cpp index 211f88f..b3807ac 100644 --- a/components/cmdscheduler/cmd_scheduler.cpp +++ b/components/cmdscheduler/cmd_scheduler.cpp @@ -25,7 +25,7 @@ void CmdScheduler::regbasiccmd() { } return (int32_t)0; }); - this->registerCmd("sleep_ms", "", 1, [this](Context* context) { + this->registerCmd("sleep_ms", "(ms)", 1, [this](Context* context) { int ms = atoi(context->argv[1]); osDelay(ms); return 0; From d3696cb389eb9dbdc808d961944bcc3a9fbc2a82 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 17:16:18 +0800 Subject: [PATCH 08/11] fix some feite servo motor bug --- components/mini_servo_motor/feite_servo_motor.cpp | 3 ++- components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp index f60b5e4..e4fe23e 100644 --- a/components/mini_servo_motor/feite_servo_motor.cpp +++ b/components/mini_servo_motor/feite_servo_motor.cpp @@ -214,7 +214,7 @@ bool FeiTeServoMotor::reCalibration(int id, int16_t pos) { int16_t nowpos; DO(getNowPos(id, nowpos)); ZLOGI(TAG, "reCalibration id:%d nowpos:%d:%d", id, nowpos, pos); - DO(setTargetPos(id, pos)); + DO(setTargetPos(id, nowpos)); return true; } @@ -355,6 +355,7 @@ bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uin } HAL_UART_DMAStop(m_uart); if (overtime_flag) { + // ZLOGE(TAG, "txandrx overtime"); return false; } return true; 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 1a88a4f..2b3cc80 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -40,7 +40,7 @@ int32_t MiniRobotCtrlModule::stop(u8 stop_type) { int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) { if (nowpos < 0 || nowpos > 4095) return err::kce_param_out_of_range; if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime; - if (m_bus->reCalibration(m_id, nowpos)) return err::kce_subdevice_overtime; + if (!m_bus->reCalibration(m_id, nowpos)) return err::kce_subdevice_overtime; return 0; } int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb) { From a796f5c582eef8b06107d7558b3dfafe6be0f639 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 20:07:04 +0800 Subject: [PATCH 09/11] update --- components/step_motor_45/script_cmder_step_motor_45.cpp | 1 + components/step_motor_45/step_motor_45.cpp | 8 +++++++- components/step_motor_45/step_motor_45.hpp | 7 +++++-- 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/components/step_motor_45/script_cmder_step_motor_45.cpp b/components/step_motor_45/script_cmder_step_motor_45.cpp index dad93a2..c6188a3 100644 --- a/components/step_motor_45/script_cmder_step_motor_45.cpp +++ b/components/step_motor_45/script_cmder_step_motor_45.cpp @@ -46,6 +46,7 @@ void ScriptCmderStepMotor45::regcmd() { REG_CMD___NO_ACK(PREFIX, zero_calibration, "(id)", 1); REG_CMD_WITH_ACK(PREFIX, get_pos, "(id)", 1, int32_t, ack); REG_CMD___NO_ACK(PREFIX, set_pos, "(id,pos)", 2, con->getInt(2)); + REG_CMD_WITH_ACK(PREFIX, get_zero_pin_state, "(id,pos)", 1, int32_t, ack); // REG_CMD___NO_ACK(PREFIX, initialize, "(id,driverPin1,driverPin2,driverPin3,driverPin4,zeroPin)", 6, con->getInt(2), con->getInt(3), con->getInt(4), con->getInt(5), con->getInt(6), con->getInt(7)); } \ No newline at end of file diff --git a/components/step_motor_45/step_motor_45.cpp b/components/step_motor_45/step_motor_45.cpp index f732a46..538aa2e 100644 --- a/components/step_motor_45/step_motor_45.cpp +++ b/components/step_motor_45/step_motor_45.cpp @@ -51,7 +51,7 @@ void StepMotor45::initialize(StepMotor45Scheduler* scheduler, cfg_t cfg) { if (cfg.zeroPin != PinNull) { m_zeroPinZGPIO = new ZGPIO(); ZASSERT(m_zeroPinZGPIO != NULL); - m_zeroPinZGPIO->initAsInput(cfg.zeroPin, ZGPIO::kMode_pullup, ZGPIO::kIRQ_noIrq, cfg.zeroPinMirror); + m_zeroPinZGPIO->initAsInput(cfg.zeroPin, cfg.ioPollType, ZGPIO::kIRQ_noIrq, cfg.zeroPinMirror); } setdriverpinstate(1, 1, 1, 1); scheduler->addMotor(this); @@ -101,6 +101,12 @@ void StepMotor45::onTimIRQ1ms() { schedule(); } +int32_t StepMotor45::get_zero_pin_state(int32_t& state) { + CriticalContext cc; + state = getzeropinstate(); + return (int32_t)0; +} + void StepMotor45::schedule() { /** * @brief 执行零点校准逻辑 diff --git a/components/step_motor_45/step_motor_45.hpp b/components/step_motor_45/step_motor_45.hpp index 42ecaa3..e848274 100644 --- a/components/step_motor_45/step_motor_45.hpp +++ b/components/step_motor_45/step_motor_45.hpp @@ -25,8 +25,9 @@ class StepMotor45 { bool enable_max_pos_limit = false; // 是否启用最大位置限位 bool mirror = false; // 是否镜像 - Pin_t zeroPin = PinNull; - bool zeroPinMirror = false; + Pin_t zeroPin = PinNull; + ZGPIO::GPIOMode_t ioPollType = ZGPIO::kMode_nopull; + bool zeroPinMirror = false; Pin_t driverPin[4] = {PinNull, PinNull, PinNull, PinNull}; bool driverPinMirror = false; @@ -79,6 +80,8 @@ class StepMotor45 { int32_t zero_calibration(); int32_t set_pos(int32_t pos) { m_pos = pos; } + int32_t get_zero_pin_state(int32_t &state); + int32_t get_pos(int32_t &pos) { pos = m_pos; return (int32_t)0; From a260a81955995cfdda6708fa0bb4a9d7c14687ca Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 22:04:15 +0800 Subject: [PATCH 10/11] fix some bug --- chip/zgpio.cpp | 20 +++++++++++++------- components/step_motor_45/step_motor_45.cpp | 9 ++++++++- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/chip/zgpio.cpp b/chip/zgpio.cpp index 2250b11..1d4ffb5 100644 --- a/chip/zgpio.cpp +++ b/chip/zgpio.cpp @@ -119,9 +119,6 @@ bool ZGPIO::enableClock() { return false; } - - - void regIRQGPIO(ZGPIO *gpio) { for (int i = 0; i < s_irqGPIO_num; i++) { if (s_irqGPIO[i] == gpio) { @@ -144,27 +141,36 @@ void ZGPIO::initAsInput(Pin_t pin, GPIOMode_t mode, GPIOIrqType_t irqtype, bool m_gpio = chip_get_gpio(pin); m_pinoff = chip_get_pinoff(pin); + uint32_t pulluptype = 0; + if (mode == kMode_nopull) { + pulluptype = GPIO_NOPULL; + } else if (mode == kMode_pullup) { + pulluptype = GPIO_PULLUP; + } else if (mode == kMode_pulldown) { + pulluptype = GPIO_PULLDOWN; + } + enableClock(); GPIO_InitTypeDef m_GPIO_InitStruct = {0}; if (m_irqtype == kIRQ_noIrq) { m_GPIO_InitStruct.Pin = m_pinoff; m_GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - m_GPIO_InitStruct.Pull = 0; + m_GPIO_InitStruct.Pull = pulluptype; m_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; } else if (m_irqtype == kIRQ_risingIrq) { m_GPIO_InitStruct.Pin = m_pinoff; m_GPIO_InitStruct.Mode = m_mirror ? GPIO_MODE_IT_FALLING : GPIO_MODE_IT_RISING; - m_GPIO_InitStruct.Pull = 0; + m_GPIO_InitStruct.Pull = pulluptype; m_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; } else if (m_irqtype == kIRQ_fallingIrq) { m_GPIO_InitStruct.Pin = m_pinoff; m_GPIO_InitStruct.Mode = !m_mirror ? GPIO_MODE_IT_FALLING : GPIO_MODE_IT_RISING; - m_GPIO_InitStruct.Pull = 0; + m_GPIO_InitStruct.Pull = pulluptype; m_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; } else if (m_irqtype == kIRQ_risingAndFallingIrq) { m_GPIO_InitStruct.Pin = m_pinoff; m_GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; - m_GPIO_InitStruct.Pull = 0; + m_GPIO_InitStruct.Pull = pulluptype; m_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; } diff --git a/components/step_motor_45/step_motor_45.cpp b/components/step_motor_45/step_motor_45.cpp index 538aa2e..b14b0ab 100644 --- a/components/step_motor_45/step_motor_45.cpp +++ b/components/step_motor_45/step_motor_45.cpp @@ -83,11 +83,14 @@ int32_t StepMotor45::stop() { } int32_t StepMotor45::zero_calibration() { CriticalContext cc; + ZLOGI(TAG, "zero_calibration1 %d", getzeropinstate()); if (getzeropinstate()) { + ZLOGI(TAG, "zero_calibration2"); m_calibration = true; m_pos = 0; return (int32_t)0; } + ZLOGI(TAG, "zero_calibration"); m_exec_zero_calibration_task = true; m_state = krollback; return (int32_t)0; @@ -96,14 +99,18 @@ int32_t StepMotor45::zero_calibration() { void StepMotor45::onTimIRQ1ms() { CriticalContext cc; if (m_state == kstop) { + m_lastzeropinstate = getzeropinstate(); return; + } else { + schedule(); } - schedule(); } int32_t StepMotor45::get_zero_pin_state(int32_t& state) { CriticalContext cc; state = getzeropinstate(); + // ZLOGI(TAG, "zero_calibration1 %d", getzeropinstate()); + return (int32_t)0; } From bf1c9dcf65962a32a6254e8e9f5517fa08844be4 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 22:11:50 +0800 Subject: [PATCH 11/11] fix some bug --- components/step_motor_45/step_motor_45.cpp | 79 +++++++++++++----------------- 1 file changed, 35 insertions(+), 44 deletions(-) diff --git a/components/step_motor_45/step_motor_45.cpp b/components/step_motor_45/step_motor_45.cpp index b14b0ab..a4a8481 100644 --- a/components/step_motor_45/step_motor_45.cpp +++ b/components/step_motor_45/step_motor_45.cpp @@ -83,14 +83,14 @@ int32_t StepMotor45::stop() { } int32_t StepMotor45::zero_calibration() { CriticalContext cc; - ZLOGI(TAG, "zero_calibration1 %d", getzeropinstate()); + // ZLOGI(TAG, "zero_calibration1 %d", getzeropinstate()); if (getzeropinstate()) { ZLOGI(TAG, "zero_calibration2"); m_calibration = true; m_pos = 0; return (int32_t)0; } - ZLOGI(TAG, "zero_calibration"); + // ZLOGI(TAG, "zero_calibration"); m_exec_zero_calibration_task = true; m_state = krollback; return (int32_t)0; @@ -130,57 +130,48 @@ void StepMotor45::schedule() { m_lastzeropinstate = iostate; } - if ((detect_rising_edge && m_state == krollback) || (detect_falling_edge && m_state == kforward_rotation)) { - /** - * @brief 触发零点 - */ - m_pos = 0; - m_calibration = true; - - if (m_exec_zero_calibration_task) { + if (m_exec_zero_calibration_task) { + if ((detect_rising_edge && m_state == krollback) || (detect_falling_edge && m_state == kforward_rotation)) { + m_pos = 0; + m_calibration = true; m_exec_zero_calibration_task = 0; stop_motor(); return; } - } - - /** - * @brief 零点限位触发,电机停止运行 - */ - if (m_cfg.enable_zero_limit && m_state == krollback) { - if (iostate) { - stop_motor(); - return; + } else { + // 触发零点 + if ((detect_rising_edge && m_state == krollback) || (detect_falling_edge && m_state == kforward_rotation)) { + m_pos = 0; + m_calibration = true; } - } - /** - * @brief 触发最大位置限位,电机停止运行 - */ - if (m_cfg.enable_max_pos_limit && m_state == kforward_rotation) { - if (m_pos >= m_cfg.max_pos) { - stop_motor(); - return; + // 零点限位触发,电机停止运行 + if (m_cfg.enable_zero_limit && m_state == krollback) { + if (iostate) { + stop_motor(); + return; + } } - } - - /** - * @brief 如果使能了最大位置限位,且电机没有校准过,则电机不允许正转 - */ - if (m_cfg.enable_max_pos_limit && !m_calibration) { - if (m_state == kforward_rotation) { - return; + // 触发最大位置限位,电机停止运行 + if (m_cfg.enable_max_pos_limit && m_state == kforward_rotation) { + if (m_pos >= m_cfg.max_pos) { + stop_motor(); + return; + } } - } - - /** - * @brief 如果运行到目标位置则停止 - */ - if (posmode) { - if (m_pos == m_targetPos) { - stop_motor(); - return; + // 如果使能了最大位置限位,且电机没有校准过,则电机不允许正转 + if (m_cfg.enable_max_pos_limit && !m_calibration) { + if (m_state == kforward_rotation) { + return; + } + } + // 如果运行到目标位置则停止 + if (posmode) { + if (m_pos == m_targetPos) { + stop_motor(); + return; + } } }