From 372b51b4c180a165ddae2544f324842fdda8d444 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 17 Oct 2023 17:32:30 +0800 Subject: [PATCH] =?UTF-8?q?step=20motor=2045=20=E6=B7=BB=E5=8A=A0=E9=9B=B6?= =?UTF-8?q?=E7=82=B9=E8=BF=87=E6=BB=A4=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- components/mini_servo_motor/feite_servo_motor.cpp | 22 +++++++++++++--------- components/step_motor_45/step_motor_45.cpp | 14 +++++++++++++- components/step_motor_45/step_motor_45.hpp | 12 ++++++++---- .../step_motor_45/step_motor_45_scheduler.cpp | 4 ++-- .../step_motor_45/step_motor_45_scheduler.hpp | 2 +- 5 files changed, 37 insertions(+), 17 deletions(-) diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp index d4336e4..d99c288 100644 --- a/components/mini_servo_motor/feite_servo_motor.cpp +++ b/components/mini_servo_motor/feite_servo_motor.cpp @@ -65,8 +65,9 @@ bool FeiTeServoMotor::getServoCalibration(uint8_t id, int16_t& poscalibration) { } run_mode_e FeiTeServoMotor::getmode(uint8_t id) { - uint8_t data = 0; - bool suc = read_u8(id, kRegServoRunMode, data); + zlock_guard l(m_mutex); + uint8_t data = 0; + bool suc = read_u8(id, kRegServoRunMode, data); if (suc) { return (run_mode_e)data; } else { @@ -205,6 +206,8 @@ void FeiTeServoMotor::dump_detailed_status(detailed_status_t* detailed_status) { bool FeiTeServoMotor::getMoveFlag(uint8_t id, uint8_t& moveflag) { return read_u8(id, kRegServoMoveFlag, moveflag); } bool FeiTeServoMotor::reCalibration(int id, int16_t pos) { + zlock_guard l(m_mutex); + if (pos < 0 || pos > 4095) { ZLOGE(TAG, "reCalibration pos:%d out of range", pos); return false; @@ -308,8 +311,8 @@ 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)); + ZASSERT((size_t)txpacketlen < sizeof(m_txbuf)); + ZASSERT((size_t)rxpacketlen < sizeof(m_rxbuf)); uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen); m_txbuf[txpacketlen - 1] = checksum; @@ -341,8 +344,8 @@ bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t l uint8_t checksum = checksum_packet((uint8_t*)cmd_header, txpacketlen); ZASSERT(txpacketlen > 0); - ZASSERT(txpacketlen < sizeof(m_txbuf)); - ZASSERT(rxpacketlen < sizeof(m_rxbuf)); + ZASSERT((size_t)txpacketlen < sizeof(m_txbuf)); + ZASSERT((size_t)rxpacketlen < sizeof(m_rxbuf)); m_txbuf[txpacketlen - 1] = checksum; if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) { @@ -359,13 +362,14 @@ 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); + // clear rx buffer + #if 1 - HAL_UART_Transmit(m_uart, tx, txdatalen, 1000); HAL_UART_Receive_DMA(m_uart, (uint8_t*)rx, expectrxsize); + HAL_UART_Transmit(m_uart, tx, txdatalen,1000); bool overtime_flag = false; - while (HAL_UART_GetState(m_uart) == HAL_UART_STATE_BUSY_RX || // - HAL_UART_GetState(m_uart) == HAL_UART_STATE_BUSY_TX_RX) { + while (HAL_UART_GetState(m_uart) != HAL_UART_STATE_READY ) { osDelay(1); int rxsize = expectrxsize - __HAL_DMA_GET_COUNTER(m_hdma_rx); if (rxsize == expectrxsize) { diff --git a/components/step_motor_45/step_motor_45.cpp b/components/step_motor_45/step_motor_45.cpp index a4a8481..5f71ad1 100644 --- a/components/step_motor_45/step_motor_45.cpp +++ b/components/step_motor_45/step_motor_45.cpp @@ -35,6 +35,9 @@ stepmotor_control_matrix_t onestepsq2[] = { void StepMotor45::initialize(StepMotor45Scheduler* scheduler, cfg_t cfg) { m_cfg = cfg; + if (m_cfg.zerofilterCount == 0) { + m_cfg.zerofilterCount = 20; + } // 初始化GPIO ZASSERT(cfg.driverPin[0] != PinNull); @@ -253,7 +256,16 @@ bool StepMotor45::getzeropinstate() { return false; } bool state = m_zeroPinZGPIO->getState(); - return state; + if (nowiostate != state) { + m_io_state_filter_count++; + if (m_io_state_filter_count > m_cfg.zerofilterCount) { + nowiostate = state; + m_io_state_filter_count = 0; + } + } else { + m_io_state_filter_count = 0; + } + return nowiostate; } void StepMotor45::setdriverpinstate(bool s1, bool s2, bool s3, bool s4) { // ZLOGI(TAG, "setdriverpinstate %d%d%d%d", s1, s2, s3, s4); diff --git a/components/step_motor_45/step_motor_45.hpp b/components/step_motor_45/step_motor_45.hpp index e848274..090d76a 100644 --- a/components/step_motor_45/step_motor_45.hpp +++ b/components/step_motor_45/step_motor_45.hpp @@ -25,9 +25,10 @@ class StepMotor45 { bool enable_max_pos_limit = false; // 是否启用最大位置限位 bool mirror = false; // 是否镜像 - Pin_t zeroPin = PinNull; - ZGPIO::GPIOMode_t ioPollType = ZGPIO::kMode_nopull; - bool zeroPinMirror = false; + Pin_t zeroPin = PinNull; + ZGPIO::GPIOMode_t ioPollType = ZGPIO::kMode_nopull; + bool zeroPinMirror = false; + int zerofilterCount = 0; Pin_t driverPin[4] = {PinNull, PinNull, PinNull, PinNull}; bool driverPinMirror = false; @@ -41,7 +42,10 @@ class StepMotor45 { bool m_lastzeropinstate; // 上一次零点限位状态 int m_acceleration; // 加速度 - bool posmode = false; + int m_io_state_filter_count = 0; + + bool posmode = false; + bool nowiostate = false; int m_targetPos; /** * @brief 控制 diff --git a/components/step_motor_45/step_motor_45_scheduler.cpp b/components/step_motor_45/step_motor_45_scheduler.cpp index eb1d6fd..4555087 100644 --- a/components/step_motor_45/step_motor_45_scheduler.cpp +++ b/components/step_motor_45/step_motor_45_scheduler.cpp @@ -3,13 +3,13 @@ #include "step_motor_45.hpp" using namespace iflytop; -void StepMotor45Scheduler::initialize(zchip_tim_t* tim, int freq) { +void StepMotor45Scheduler::initialize(zchip_tim_t* tim) { m_htim = tim; uint32_t prescaler = 0; uint32_t autoreload = 0; - ZASSERT(chip_calculate_prescaler_and_autoreload_by_expect_freq(chip_get_timer_clock_sorce_freq(m_htim), freq, &prescaler, &autoreload)); + ZASSERT(chip_calculate_prescaler_and_autoreload_by_expect_freq(chip_get_timer_clock_sorce_freq(m_htim), 1000, &prescaler, &autoreload)); HAL_TIM_Base_DeInit(m_htim); m_htim->Init.Prescaler = prescaler; diff --git a/components/step_motor_45/step_motor_45_scheduler.hpp b/components/step_motor_45/step_motor_45_scheduler.hpp index 02431a4..245c6e7 100644 --- a/components/step_motor_45/step_motor_45_scheduler.hpp +++ b/components/step_motor_45/step_motor_45_scheduler.hpp @@ -12,7 +12,7 @@ class StepMotor45Scheduler { list m_motor_list; public: - void initialize(zchip_tim_t* tim, int freq); + void initialize(zchip_tim_t* tim); void addMotor(StepMotor45* motor); void start();