From 54ca130e8ef91082a9ad7370d1b94b57c6dfe5b6 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 6 Sep 2023 00:27:30 +0800 Subject: [PATCH] add step_motor_45 --- components/step_motor_45/step_motor_45.cpp | 197 +++++++++++++++++++++ components/step_motor_45/step_motor_45.hpp | 74 ++++++++ .../step_motor_45/step_motor_45_scheduler.cpp | 38 ++++ .../step_motor_45/step_motor_45_scheduler.hpp | 24 +++ 4 files changed, 333 insertions(+) create mode 100644 components/step_motor_45/step_motor_45.cpp create mode 100644 components/step_motor_45/step_motor_45.hpp create mode 100644 components/step_motor_45/step_motor_45_scheduler.cpp create mode 100644 components/step_motor_45/step_motor_45_scheduler.hpp diff --git a/components/step_motor_45/step_motor_45.cpp b/components/step_motor_45/step_motor_45.cpp new file mode 100644 index 0000000..544e2b6 --- /dev/null +++ b/components/step_motor_45/step_motor_45.cpp @@ -0,0 +1,197 @@ +#include "step_motor_45.hpp" + +#include + +using namespace iflytop; +#define TAG "StepMotor45" + +typedef struct { + uint8_t v[4]; +} stepmotor_control_matrix_t; + +stepmotor_control_matrix_t onestepsq1[] = { + {{0, 1, 1, 1}}, // 0 + {{0, 0, 1, 1}}, // 1 + {{1, 0, 1, 1}}, // 2 + {{1, 0, 0, 1}}, // 3 + {{1, 1, 0, 1}}, // 4 + {{1, 1, 0, 0}}, // 5 + {{1, 1, 1, 0}}, // 6 + {{0, 1, 1, 0}}, // 7 +}; + +stepmotor_control_matrix_t onestepsq2[] = { + {{0, 1, 1, 0}}, // 7 + {{1, 1, 1, 0}}, // 6 + {{1, 1, 0, 0}}, // 5 + {{1, 1, 0, 1}}, // 4 + {{1, 0, 0, 1}}, // 3 + {{1, 0, 1, 1}}, // 2 + {{0, 0, 1, 1}}, // 1 + {{0, 1, 1, 1}}, // 0 +}; + +#define MAX_STEP_TABLE_OFF (ZARRAY_SIZE(onestepsq1) - 1) + +void StepMotor45::initialize(cfg_t cfg) { + m_cfg = cfg; + + // 初始化GPIO + ZASSERT(cfg.driverPin[0] != PinNull); + ZASSERT(cfg.driverPin[1] != PinNull); + ZASSERT(cfg.driverPin[2] != PinNull); + ZASSERT(cfg.driverPin[3] != PinNull); + + for (size_t i = 0; i < 4; i++) { + m_driverPinZGPIO[i] = new ZGPIO(); + ZASSERT(m_driverPinZGPIO[i] != NULL); + m_driverPinZGPIO[i]->initAsOutput(cfg.driverPin[i], ZGPIO::kMode_nopull, cfg.driverPinMirror, true); + } + + 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); + } + return; +} + +void StepMotor45::rotate(bool direction, int speed) { + CriticalContext cc; + if (direction) { + if (getzeropinstate()) { + m_calibration = true; + m_pos = 0; + } + } + m_state = direction ? kforward_rotation : krollback; +} +void StepMotor45::stop() { + CriticalContext cc; + stop_motor(); +} +void StepMotor45::zeroCalibration() { + CriticalContext cc; + if (getzeropinstate()) { + m_calibration = true; + m_pos = 0; + return; + } + m_exec_zero_calibration_task = true; + m_state = krollback; +} + +void StepMotor45::onTimIRQ1ms() { + CriticalContext cc; + if (m_state == kstop) { + return; + } + schedule(); +} + +void StepMotor45::schedule() { + /** + * @brief 执行零点校准逻辑 + */ + bool iostate = getzeropinstate(); + bool detect_rising_edge = false; + bool detect_falling_edge = false; + if (m_lastzeropinstate != iostate) { + if (iostate) { + detect_rising_edge = true; + } else { + detect_falling_edge = true; + } + 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) { + m_exec_zero_calibration_task = 0; + stop_motor(); + return; + } + } + + /** + * @brief 零点限位触发，电机停止运行 + */ + if (m_cfg.enable_zero_limit && m_state == krollback) { + if (iostate) { + stop_motor(); + return; + } + } + + /** + * @brief 触发最大位置限位，电机停止运行 + */ + if (m_cfg.enable_max_pos_limit && m_state == kforward_rotation) { + if (m_pos >= m_cfg.max_pos) { + stop_motor(); + return; + } + } + + /** + * @brief 如果使能了最大位置限位，且电机没有校准过，则电机不允许正转 + */ + if (m_cfg.enable_max_pos_limit && !m_calibration) { + if (m_state == kforward_rotation) { + return; + } + } + + /** + * @brief + * 按照相位表执行电机驱动 + */ + int direction = 0; + if (m_state == kforward_rotation) { + direction = 1; + } else if (m_state == krollback) { + direction = -1; + } + + m_off += direction; + if (m_off < 0) { + m_off = MAX_STEP_TABLE_OFF; + } else if (m_off > MAX_STEP_TABLE_OFF) { + m_off = 0; + } + + stepmotor_control_matrix_t* table = NULL; + if (m_cfg.mirror) { + table = onestepsq2; + } else { + table = onestepsq1; + } + + setdriverpinstate(table[m_off].v[0], table[m_off].v[1], table[m_off].v[2], table[m_off].v[3]); + + m_pos += direction; +} + +bool StepMotor45::getzeropinstate() { + if (!m_zeroPinZGPIO) { + return false; + } + bool state = m_zeroPinZGPIO->getState(); + return state; +} +void StepMotor45::setdriverpinstate(bool s1, bool s2, bool s3, bool s4) { + ZLOGI(TAG, "setdriverpinstate %d%d%d%d", s1, s2, s3, s4); + + m_driverPinZGPIO[0]->setState(s1); + m_driverPinZGPIO[1]->setState(s2); + m_driverPinZGPIO[2]->setState(s3); + m_driverPinZGPIO[3]->setState(s4); +} + +void StepMotor45::stop_motor() { m_state = kstop; } diff --git a/components/step_motor_45/step_motor_45.hpp b/components/step_motor_45/step_motor_45.hpp new file mode 100644 index 0000000..48ad869 --- /dev/null +++ b/components/step_motor_45/step_motor_45.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include "sdk/os/zos.hpp" + +namespace iflytop { +using namespace std; + +/** + * @brief 四线五项步进电机 + * https://iflytop1.feishu.cn/wiki/MtaawWm8yijb0tkpWoic1IhsnZc + */ + +class StepMotor45 { + public: + typedef enum { + kstop, + kforward_rotation, + krollback, + } state_t; + + typedef struct { + int max_pos = INT32_MAX; // 最大位置 + bool enable_zero_limit = false; // 是否启用零点限位 + bool enable_max_pos_limit = false; // 是否启用最大位置限位 + bool mirror = false; // 是否镜像 + + Pin_t zeroPin = PinNull; + bool zeroPinMirror = false; + + Pin_t driverPin[4] = {PinNull, PinNull, PinNull, PinNull}; + bool driverPinMirror = false; + } cfg_t; + + private: + int m_off; // 相位偏移 + int m_pos; // 当前位置 + bool m_calibration; // 校准状态,是否已经校准 + state_t m_state; // 当前状态 + bool m_lastzeropinstate; // 上一次零点限位状态 + int m_acceleration; // 加速度 + /** + * @brief 控制 + */ + // 执行零点校准任务,如果执行，则电机会自动停止 + bool m_exec_zero_calibration_task; + + /** + * @brief 配置 + */ + + ZGPIO *m_driverPinZGPIO[4]; + ZGPIO *m_zeroPinZGPIO; + + cfg_t m_cfg; + + public: + void initialize(cfg_t cfg); + + void rotate(bool direction, int speed); + void stop(); + void zeroCalibration(); + + private: + bool getzeropinstate(); + void setdriverpinstate(bool s1, bool s2, bool s3, bool s4); + + void schedule(); + void stop_motor(); + + public: + void onTimIRQ1ms(); +}; + +} // namespace iflytop \ No newline at end of file diff --git a/components/step_motor_45/step_motor_45_scheduler.cpp b/components/step_motor_45/step_motor_45_scheduler.cpp new file mode 100644 index 0000000..a50e707 --- /dev/null +++ b/components/step_motor_45/step_motor_45_scheduler.cpp @@ -0,0 +1,38 @@ +#include "step_motor_45_scheduler.hpp" + +using namespace iflytop; + +void StepMotor45Scheduler::initialize(zchip_tim_t* tim, int freq) { + 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)); + + HAL_TIM_Base_DeInit(m_htim); + m_htim->Init.Prescaler = prescaler; + m_htim->Init.CounterMode = TIM_COUNTERMODE_UP; + m_htim->Init.Period = autoreload; + m_htim->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + HAL_TIM_Base_Init(m_htim); + + HAL_NVIC_SetPriority(chip_tim_get_irq(m_htim), IFLYTOP_PREEMPTPRIORITY_DEFAULT, 0); + HAL_NVIC_EnableIRQ(chip_tim_get_irq(m_htim)); + + // HAL_TIM_Base_Start_IT(m_htim); + + ZIRQDispatcher::instance().regTimIrqListener([this](zchip_tim_t* tim) { + if (tim != m_htim) return; + loop(); + }); +} + +void StepMotor45Scheduler::start() { HAL_TIM_Base_Start_IT(m_htim); } +void StepMotor45Scheduler::addMotor(StepMotor45* motor) { m_motor_list.push_back(motor); } + +void StepMotor45Scheduler::loop() { + for (auto motor : m_motor_list) { + motor->onTimIRQ1ms(); + } +} \ No newline at end of file diff --git a/components/step_motor_45/step_motor_45_scheduler.hpp b/components/step_motor_45/step_motor_45_scheduler.hpp new file mode 100644 index 0000000..d170e0b --- /dev/null +++ b/components/step_motor_45/step_motor_45_scheduler.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include "sdk/os/zos.hpp" +#include "step_motor_45.hpp" +namespace iflytop { +using namespace std; + +class StepMotor45Scheduler { + public: + zchip_tim_t* m_htim; + + list m_motor_list; + + public: + void initialize(zchip_tim_t* tim, int freq); + void addMotor(StepMotor45* motor); + + void start(); + + private: + void loop(); +}; + +} // namespace iflytop \ No newline at end of file