update

dep/libtrinamic

@@ -1 +1 @@
-Subproject commit d4381558b25522ebd43c86d125f8177d38c578e3
+Subproject commit b0450ea2b7b4892637b4885c2518d3b8db245588

src/board/hardware.cpp

@@ -213,7 +213,7 @@ void Hardware::STM32_HAL_onGPIO_EXTI_Callback(uint16_t gpioNum) {
   if (gpioNum == TMC_HOME_REF_GPIO_PIN) {
     if (STM32_HAL_GPIO_IS_THIS_PIN_TRIGGER_IRQ(TMC_HOME_REF_GPIO)) {
       for (size_t i = 0; i < m_listenerNum; i++) {
-        m_listener[i]->Hardware_onHomeRefSwitch();
+        m_listener[i]->Hardware_onHomeRefSwitchISR();
       }
     }
   }

src/board/hardware.hpp

@@ -18,7 +18,7 @@ namespace iflytop {
 class HardwareListener {
  public:
   virtual void Hardware_OnHardwareException(){};
-  virtual void Hardware_onHomeRefSwitch(){};
+  virtual void Hardware_onHomeRefSwitchISR(){};
 };
 
 class Hardware : public IflytopCanSlaveListener,  //
@@ -28,16 +28,14 @@ class Hardware : public IflytopCanSlaveListener,  //
  private:
   /* data */
 
- public:
+ private:
   TMC4361A        tmc4361motor1;
   TMP117          tmp117[4];
   FanStateMonitor m_fanStateMonitor[6];
   IflytopCanSlave canSlaveService;
   bool            m_canOnRxDataFlag;
-
   /* data */
-  bool m_fanState[6];
-
+  bool              m_fanState[6];
   HardwareListener *m_listener[MAX_HARDWARE_LISTENER_NUM];
   int               m_listenerNum;
 
@@ -60,7 +58,8 @@ class Hardware : public IflytopCanSlaveListener,  //
   void hardwareinit();
   void periodicJob();
 
-  int32_t port_tmc4361_get_version(uint8_t channel);
+  int32_t   port_tmc4361_get_version(uint8_t channel);
+  TMC4361A *getMotor1() { return &tmc4361motor1; }
 
   /*******************************************************************************
    *                                    测试                                     *

src/board/project_board.hpp

@@ -22,4 +22,9 @@
 
 #define TMC_HOME_REF_GPIO_PIN      GPIO_PIN_3
 #define TMC_HOME_REF_GPIO_PORT     GPIOD
-#define TMC_HOME_REF_GPIO_IRQ_MODE GPIO_MODE_IT_RISING
+#define TMC_HOME_REF_GPIO_IRQ_MODE GPIO_MODE_IT_RISING
+
+typedef enum {
+  kok = 0,
+  kGoHomeFail,
+} exception_id_t;

src/lncubator_rotating_control_service.cpp

@@ -3,67 +3,196 @@
 using namespace iflytop;
 #define TAG "LRCS"
 
-const uint32_t LncubatorRotatingControlService::mc_onecircle_ustep_num = (256 * 200 * 50);
-const uint32_t LncubatorRotatingControlService::mc_default_velocity    = 500000;  // 500k
+const uint32_t LncubatorRotatingControlService::mc_onecircle_ustep_num    = (256 * 200 * 50);
+const uint32_t LncubatorRotatingControlService::mc_default_velocity       = 500000;  // 500kpps
+const uint32_t LncubatorRotatingControlService::mc_one_sub_position_width = (256 * 200 * 50) / 20;
 
 LncubatorRotatingControlService::LncubatorRotatingControlService() {}
 LncubatorRotatingControlService::~LncubatorRotatingControlService() {}
-
-/**
- * @brief
- *
- *
- * 1. 定位
- * 2. 标记零点位置
- * 3.
- *
- */
-
 void LncubatorRotatingControlService::initialize(Hardware* hardware) {  //
-  m_hardware     = hardware;
-  m_motor        = &m_hardware->tmc4361motor1;
-  m_os           = m_hardware;
-  m_workfinished = false;
-  m_dowhat       = kidle;
-  m_maxVelocity  = mc_default_velocity;
+  m_hardware       = hardware;
+  m_motor          = m_hardware->getMotor1();
+  m_os             = m_hardware;
+  m_workfinished   = false;
+  m_dowhat         = kidle;
+  m_maxVelocity    = mc_default_velocity;
+  m_inletPosition  = 0;
+  m_outletPosition = m_inletPosition + mc_one_sub_position_width / 2;
 
   /**
    * @brief 电机必须工作在位置模式
    */
   m_motor->setMotorInPositionMode();
   hardware->registerListener(this);
+  m_motor->registerListener(this, TMC4361A::event_mask_t::ktarget_reached);
 }
-void LncubatorRotatingControlService::moveToHome() {
-  m_dowhat = krotateToHome;
-  m_motor->moveTo(mc_onecircle_ustep_num * 2, m_maxVelocity);
+
+void LncubatorRotatingControlService::setVelocity(int32_t velocityMax) { m_maxVelocity = velocityMax; }
+void LncubatorRotatingControlService::registerListener(LncubatorRotatingControlServiceListener* listener) {  //
+  m_listener = listener;
 }
-void LncubatorRotatingControlService::Hardware_onHomeRefSwitch() {
+
+/*******************************************************************************
+ *                                  motorHelp                                  *
+ *******************************************************************************/
+int32_t LncubatorRotatingControlService::tmc_getPosition() {
+  int32_t position = m_motor->getENC_POS();
+  return normalizePosition(position);
+}
+void LncubatorRotatingControlService::tmc_roateToPosition(int32_t position) {
+  int32_t _position = normalizePosition(position);
+  m_motor->moveTo(position, m_maxVelocity);
+}
+int32_t LncubatorRotatingControlService::normalizePosition(int32_t position) {
+  while (true) {
+    if (position > mc_onecircle_ustep_num / 2)
+      position = position - mc_onecircle_ustep_num;
+    else if (position < mc_onecircle_ustep_num / 2)
+      position = position + mc_onecircle_ustep_num;
+    else
+      break;
+  }
+  if (position == mc_onecircle_ustep_num / 2) position = position - 1;
+  if (position == -mc_onecircle_ustep_num / 2) position = position + 1;
+  return position;
+}
+void LncubatorRotatingControlService::tmc_moveBy(int32_t relativePosition) { m_motor->moveBy(relativePosition, m_maxVelocity); }
+/*******************************************************************************
+ *                                TriggerEvent                                 *
+ *******************************************************************************/
+void LncubatorRotatingControlService::Hardware_onHomeRefSwitchISR() {
   /**
    * @brief 电机零点光电触发
    */
   m_homeKeyTriggered = true;
 }
-void LncubatorRotatingControlService::setVelocity(uint32_t velocityMax) { m_maxVelocity = velocityMax; }
-void LncubatorRotatingControlService::rotate(uint32_t count) { m_motor->moveBy(count, m_maxVelocity); }
+void LncubatorRotatingControlService::triggerEvent(interevent_t event) {}
+
 void LncubatorRotatingControlService::periodicJob() {
-  if (krotateToHome == m_dowhat) {
-    doRotateToHomeJob();
+  if (kmoveToHomeJob == m_dowhat) {
+    doRotateToHomeJob(kPeriodicJobEvent);
+  } else if (kmoveSubPositionJob == m_dowhat) {
+    doMoveToSubPositionJob(kPeriodicJobEvent);
+  } else if (kmoveByJob == m_dowhat) {
+    doMoveByJob(kPeriodicJobEvent);
   }
-}
-
-void LncubatorRotatingControlService::doRotateToHomeJob() {
   if (m_homeKeyTriggered) {
     m_homeKeyTriggered = false;
+    if (kmoveToHomeJob == m_dowhat) {
+      doRotateToHomeJob(kPeriodicJobEvent);
+    } else if (kmoveSubPositionJob == m_dowhat) {
+      doMoveToSubPositionJob(kPeriodicJobEvent);
+    } else if (kmoveByJob == m_dowhat) {
+      doMoveByJob(kPeriodicJobEvent);
+    }
+  }
+}
+void LncubatorRotatingControlService::TMC4361A_onEvent(uint32_t event) {
+  if (event & TMC4361A::event_mask_t::ktarget_reached) {
+    if (kmoveToHomeJob == m_dowhat) {
+      doRotateToHomeJob(kTMC4361AReachTargetEvent);
+    } else if (kmoveSubPositionJob == m_dowhat) {
+      doMoveToSubPositionJob(kTMC4361AReachTargetEvent);
+    } else if (kmoveByJob == m_dowhat) {
+      doMoveByJob(kTMC4361AReachTargetEvent);
+    }
+  }
+}
+/*******************************************************************************
+ *                               RotateToHomeJob                               *
+ *******************************************************************************/
+
+void LncubatorRotatingControlService::moveToHome() {
+  if (m_dowhat != kidle) return;
+  m_dowhat = kmoveToHomeJob;
+  moveBy(mc_onecircle_ustep_num * 2);
+}
+void LncubatorRotatingControlService::doRotateToHomeJob(interevent_t event) {
+  if (event == kPeriodicJobEvent) {
+    if (m_homeKeyTriggered) {
+      m_homeKeyTriggered = false;
+      /**
+       * @brief 零点光电触发,记录零点位置
+       */
+      m_motor->stop();
+      m_motor->setZeroPoint();
+      m_dowhat = kidle;
+      ZLOGI(TAG, "find Zero Point");
+      if (m_listener) m_listener->LncubatorRotatingControlService_onMoveToHomeJobFinished();
+    }
+  } else if (event == kTMC4361AReachTargetEvent) {
     /**
-     * @brief 零点光电触发,记录零点位置
+     * @brief 电机转了两圈依然没有找到零点
      */
-    m_homePosition = m_motor->getENC_POS();
-    m_motor->stop();
+    if (m_listener) m_listener->LncubatorRotatingControlService_onException(kGoHomeFail);
+    m_dowhat = kidle;
+  }
+}
+void LncubatorRotatingControlService::calibrationPosition(refpoint_t refpoint) {
+  /**
+   * @brief 此时是孵育船1对其了入口
+   */
+  if (refpoint == REFPOINT_TYPE_INLET) {
+    m_inletPosition  = tmc_getPosition();
+    m_outletPosition = normalizePosition(m_inletPosition + mc_onecircle_ustep_num / 2);
+  }
+}
+
+/*******************************************************************************
+ *                                  moveToSubPosition                          *
+ *******************************************************************************/
+void LncubatorRotatingControlService::moveToSubPosition(refpoint_t refpoint, int32_t positionIndex) {
+  if (m_dowhat != kidle) return;
+  m_dowhat = kmoveSubPositionJob;
+  // 出口
+  if (refpoint == REFPOINT_TYPE_OUTLET) {
+    int32_t targetPosition = m_outletPosition + positionIndex * mc_one_sub_position_width;
+    tmc_roateToPosition(targetPosition);
+  } else if (refpoint == REFPOINT_TYPE_INLET) {
+    int32_t targetPosition = m_inletPosition + positionIndex * mc_one_sub_position_width;
+    tmc_roateToPosition(targetPosition);
+  } else {
+    ZLOGE(TAG, "refpoint error");
+  }
+}
+void LncubatorRotatingControlService::doMoveToSubPositionJob(interevent_t event) {
+  if (event == kPeriodicJobEvent) {
+  } else if (event == kTMC4361AReachTargetEvent) {
+    /**
+     * @brief moveToTarget
+     */
+    if (m_listener) m_listener->LncubatorRotatingControlService_onMoveSubPositionJobFinished();
     m_dowhat = kidle;
-    ZLOGI(TAG, "home position:%d", m_homePosition);
   }
 }
 
-void LncubatorRotatingControlService::registerListener(LncubatorRotatingControlServiceListener* listener) {  //
-  m_listener = listener;
+/*******************************************************************************
+ *                                  moveByJob                                  *
+ *******************************************************************************/
+void LncubatorRotatingControlService::moveBy(int32_t pluseCount) {
+  if (m_dowhat == kidle || m_dowhat == kmoveByJob) {
+    m_dowhat = kmoveByJob;
+    tmc_moveBy(pluseCount);
+  }
+}
+void LncubatorRotatingControlService::doMoveByJob(interevent_t event) {
+  if (event == kPeriodicJobEvent) {
+  } else if (event == kTMC4361AReachTargetEvent) {
+    /**
+     * @brief moveToTarget
+     */
+    if (m_listener) m_listener->LncubatorRotatingControlService_onMoveByJobFinished();
+    m_dowhat = kidle;
+  }
+}
+/*******************************************************************************
+ *                                    STOP                                     *
+ *******************************************************************************/
+void LncubatorRotatingControlService::stop() {
+  if (m_dowhat == kidle) return;
+  if (m_dowhat == kmoveToHomeJob) {
+    m_motor->stop();
+    m_dowhat = kidle;
+    return;
+  }
 }

src/lncubator_rotating_control_service.hpp

@@ -2,6 +2,7 @@
 #include "board\hardware.hpp"
 #include "libiflytop_micro\stm32\basic\stm32_hal.hpp"
 #include "libiflytop_micro\stm32\component\state_machine\state_machine.hpp"
+#include "libtrinamic\src\ic\tmc4361A.hpp"
 
 namespace iflytop {
 /**
@@ -17,84 +18,114 @@ namespace iflytop {
  * 7. 读取校准值
  * 8. 写入校准值
  *
+ * 约定:
+ *   一圈位置的位置为 (-256*200/2)<0<(-256*200/2)
+ *
  *
  *
  */
 
 #define LNCUBATOR_MAX_POSITION_COUNT 20
 #define ZEROOFFSETCORRECTION         0
+#define FIND_HOME_POINT_TIMEOUT_MS   (2 * 60 * 1000)
 
 class LncubatorRotatingControlServiceListener {
  public:
-  virtual void OnlncubatorRotatingControlServiceException()   = 0;
-  virtual void OnlncubatorRotatingControlServiceJobFinished() = 0;
+  virtual void LncubatorRotatingControlService_onException(exception_id_t exception) = 0;
+  virtual void LncubatorRotatingControlService_onMoveToHomeJobFinished()             = 0;
+  virtual void LncubatorRotatingControlService_onMoveSubPositionJobFinished()        = 0;
+  virtual void LncubatorRotatingControlService_onMoveByJobFinished()                 = 0;
 };
 
-class LncubatorRotatingControlService : public HardwareListener {
+class LncubatorRotatingControlService : public HardwareListener,  //
+                                        public TMC4361AListener {
  public:
   typedef enum {
     REFPOINT_TYPE_OUTLET = 0,  // 出口
     REFPOINT_TYPE_INLET,       // 入口
     REFPOINT_TYPE_ZERO,        // 零点
   } refpoint_t;
-
+  /*******************************************************************************
+   *                                  常量配置                                   *
+   *******************************************************************************/
   static const uint32_t mc_onecircle_ustep_num;
   static const uint32_t mc_default_velocity;
+  static const uint32_t mc_one_sub_position_width;
 
  private:
   typedef enum {
     kidle,
-    krotateToTarget,
-    krotateToHome,
-    kspeedMode,
+    kmoveSubPositionJob,
+    kmoveToHomeJob,
+    kmoveByJob,
   } dowhat_t;
+  typedef enum {
+    kPeriodicJobEvent,
+    kHomeSwitchTriggeredEvent,
+    kTMC4361AReachTargetEvent,
+  } interevent_t;
+
   /* data */
   Hardware*       m_hardware;
   IflytopMicroOS* m_os;
   TMC4361A*       m_motor;
   dowhat_t        m_dowhat;
 
-  // 零点位置
-  uint32_t m_homePosition;
+  int32_t m_inletPosition;   // 入口对其第0个孵育船的位置
+  int32_t m_outletPosition;  // 出口对其第0个孵育船的位置,出口理论上应该是入口+
+
   // 便宜修正，第一个仓位中心距离零点的距离
-  uint32_t m_homeOffsetCorrection;
-  uint32_t m_maxVelocity = 0;
-  bool     m_workfinished;
-  bool     m_homeKeyTriggered;
+  int32_t m_maxVelocity = 0;
+  bool    m_workfinished;
+  bool    m_homeKeyTriggered;
 
   LncubatorRotatingControlServiceListener* m_listener;
 
- private:
  public:
   LncubatorRotatingControlService();
   ~LncubatorRotatingControlService();
 
   void initialize(Hardware* hardware);
-  void setVelocity(uint32_t velocityMax);
-  void rotate(uint32_t count);
-
   void registerListener(LncubatorRotatingControlServiceListener* listener);
+  void periodicJob();
+
+  void    setVelocity(int32_t velocityMax);
+  void    setInletPositionAndOutletPosition(int32_t inletPosition, int32_t outletPosition);
+  int32_t getVelocity() { return m_maxVelocity; }
+  int32_t getInletPosition() { return m_inletPosition; }
+  int32_t getOutletPosition() { return m_outletPosition; }
 
   void moveToHome();
-  void moveTo(refpoint_t refpoint, uint32_t positionIndex);
+  void moveBy(int32_t pluseCount);
+  void moveToSubPosition(refpoint_t refpoint, int32_t positionIndex);
   void stop();
-
   /**
    * @brief 人工校准当前位置为指定位置
    *
    * @param refpoint 参考点
    * @param positionIndex 第几个位置
    */
-  void calibrationPosition(refpoint_t refpoint, uint32_t positionIndex);
-
-  void         periodicJob();
+  void calibrationPosition(refpoint_t refpoint);
 
   /*******************************************************************************
    *                          OVERRIDE HardwareListener                          *
    *******************************************************************************/
-  virtual void Hardware_onHomeRefSwitch();
+  virtual void Hardware_onHomeRefSwitchISR();
+  /*******************************************************************************
+   *                          OVERRIDE TMC4361AListener                          *
+   *******************************************************************************/
+  virtual void TMC4361A_onEvent(uint32_t event);
+
+ private:
+  void triggerEvent(interevent_t event);
+  void doRotateToHomeJob(interevent_t event);
+  void doMoveToSubPositionJob(interevent_t event);
+  void doMoveByJob(interevent_t event);
 
  private:
-  void doRotateToHomeJob();
+  int32_t tmc_getPosition();
+  void    tmc_roateToPosition(int32_t position);
+  void    tmc_moveBy(int32_t position);
+  int32_t normalizePosition(int32_t position);
 };
 }  // namespace iflytop