add step_motor_45

components/step_motor_45/step_motor_45.cpp

@@ -0,0 +1,197 @@
+#include "step_motor_45.hpp"
+
+#include <stdint.h>
+
+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; }

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

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();
+  }
+}

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<StepMotor45*> m_motor_list;
+
+ public:
+  void initialize(zchip_tim_t* tim, int freq);
+  void addMotor(StepMotor45* motor);
+
+  void start();
+
+ private:
+  void loop();
+};
+
+}  // namespace iflytop