update

components/pipette_module/pipette_ctrl_module_v2.cpp

@@ -25,7 +25,7 @@ using namespace iflytop;
   }
 
 #define PIPETTE_PREPARE_POS 800
-#define LLF_DPOS 1000
+#define LLF_DPOS            1000
 
 void PipetteModule::initialize(int32_t id, config_t *config, StepMotorCtrlModule *zmotor, hardward_config_t *hardwaredcfg) {  //
   ZASSERT(config != nullptr);
@@ -43,8 +43,8 @@ void PipetteModule::initialize(int32_t id, config_t *config, StepMotorCtrlModule
   m_config.aspirate_distribut_pump_vel = 500;  //
   m_config.pump_vmax                   = 1000;
   m_config.lld_pump_vel                = 100;  // lld推荐使用速度为50->200,这里
-  m_config.lld_motor_vel_rpm           = 50;
-  m_config.lld_detect_period_ms        = 100;  // 100ms
+  m_config.lld_motor_vel_rpm           = 100;
+  m_config.lld_detect_period_ms        = 30;  // 100ms
   m_config.lld_prepare_pos             = PIPETTE_PREPARE_POS;
   m_config.lld_prepare_distribut_pos   = 100;
 
@@ -157,7 +157,7 @@ int32_t PipetteModule::pipette_distribut(int32_t ul) {
 int32_t PipetteModule::pipette_shake_up(int32_t ul, int32_t times) {
   ZLOGI(TAG, "pipette_shake_up %d %d", ul, times);
   if (creg.module_status == 1) return err::kdevice_is_busy;
-  return 0;
+  return do_pipette_shake_up(ul, 0, times);
 }
 
 int32_t PipetteModule::pipette_lld_is_detect_liquid(int32_t *detect_liquid) {  //
@@ -177,7 +177,7 @@ int32_t PipetteModule::pipette_distribut_llf(int32_t ul, int32_t zmotor_v) {  //
 }
 int32_t PipetteModule::pipette_shake_up_llf(int32_t ul, int32_t zmotor_v, int32_t times) {  //
   ZLOGI(TAG, "pipette_shake_up_llf %d %d %d %d", ul, zmotor_v, times);
-  return 0;
+  return do_pipette_shake_up(ul, zmotor_v, times);
 }
 
 /***********************************************************************************************************************
@@ -542,7 +542,7 @@ int32_t PipetteModule::do_pipette_distribut(int32_t ul, int32_t zmotor_v) {
   m_thread.stop();
   creg.module_status = 1;
   m_thread.start(
-    
+
       [this, ul, zmotor_v]() {  //
         auto submotor = m_zmotor->getMotor();
         pipette_enable_zmotor(1);
@@ -578,6 +578,107 @@ int32_t PipetteModule::do_pipette_distribut(int32_t ul, int32_t zmotor_v) {
   return 0;
 }
 
+int32_t PipetteModule::do_pipette_shake_up(int32_t ul, int32_t zmotor_v, int32_t times) {
+  /**
+   * @brief
+   */
+
+  ZLOGI(TAG, "do_pipette_shake_up %d", ul);
+
+  if (m_state.load_val_ul != 0) {  // 移液枪摇匀必须为空
+    return err::kpipette_error_pump_load_val_is_not_empty;
+  }
+
+  m_thread.stop();
+  creg.module_status = 1;
+  m_thread.start(
+
+      [this, ul, zmotor_v,times]() {  //
+        auto submotor = m_zmotor->getMotor();
+        pipette_enable_zmotor(1);
+        DO_IN_THREAD(befor_run());
+        int32_t dpos = 0;
+
+        int32_t startpos = 0;
+        int32_t endpos   = 0;
+        int32_t dotimes    = 0;
+
+        // 吸液
+        startpos = submotor->getXACTUAL();
+        if (zmotor_v != 0) submotor->moveBy(LLF_DPOS, zmotor_v);
+        DO_IN_THREAD(m_smtp2.pump_aspirate(m_config.aspirate_distribut_pump_vel, ul));
+        while (true) {
+          if (!check_when_run()) break;
+          if (m_thread.getExitFlag()) break;
+
+          int32_t isbusy = 0;
+          DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy));
+          if (isbusy == 0) break;
+
+          m_thread.sleep(10);
+        }
+        endpos = submotor->getXACTUAL();
+        dpos   = abs(endpos - startpos);
+
+        while (true) {
+          // 分配
+          ZLOGI(TAG, "pump_distribut ...");
+          DO_IN_THREAD(m_smtp2.pump_distribut(m_config.aspirate_distribut_pump_vel, ul));
+          if (zmotor_v != 0) submotor->moveBy(-dpos, zmotor_v);
+          while (true) {
+            if (!check_when_run()) return;
+            if (m_thread.getExitFlag()) return;
+
+            int32_t isbusy             = 0;
+            bool    motorIsReachTarget = false;
+            DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy));
+            motorIsReachTarget = submotor->isReachTarget();
+
+            if (isbusy == 0 && motorIsReachTarget) break;
+
+            m_thread.sleep(10);
+          }
+          dotimes++;
+          if (dotimes >= times) {
+            ZLOGI(TAG, "shake end ...")
+            break;
+          }
+
+          // 吸收
+          ZLOGI(TAG, "pump_aspirate ...");
+          if (zmotor_v != 0) submotor->moveBy(dpos, zmotor_v);
+          DO_IN_THREAD(m_smtp2.pump_aspirate(m_config.aspirate_distribut_pump_vel, ul));
+          while (true) {
+            if (!check_when_run()) return;
+            if (m_thread.getExitFlag()) return;
+
+            int32_t isbusy             = 0;
+            bool    motorIsReachTarget = false;
+            DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy));
+            motorIsReachTarget = submotor->isReachTarget();
+
+            if (isbusy == 0 && motorIsReachTarget) break;
+
+            m_thread.sleep(10);
+          }
+        }
+
+      },
+      [this]() {  // exit fn
+        after_run();
+        m_smtp2.pump_stop();
+        m_zmotor->getMotor()->stop();
+
+        m_state.load_val_ul = m_state.load_val_ul + m_state.dul;
+        if (m_state.load_val_ul < 0) {
+          m_state.load_val_ul = 0;
+        }
+        ZLOGI(TAG, "do_pipette_distribut finish, load_val_ul:%d");
+      }  //
+  );
+  return 0;
+}
+
 int32_t PipetteModule::pipette_enable_zmotor(int32_t enable) { return m_zmotor->step_motor_enable(enable); }
 int32_t PipetteModule::pipette_get_sensor_sample_data(int32_t index, int32_t *motor_pos, int32_t *cval, int32_t *pval) {
   if (index > capturedata_num) return err::kparam_out_of_range;
@@ -656,33 +757,16 @@ void PipetteModule::push_snesor_sample_data(int32_t motor_pos, int32_t cap_val,
 /*******************************************************************************
  *                                   PRIVATE                                   *
  *******************************************************************************/
-int32_t PipetteModule::read_pos_ul(int32_t *val) {
+int32_t PipetteModule::read_pos_ul(int32_t *val) {  //
+  *val = m_state.load_val_ul;
   return 0;
-  //  return m_smtp2.read_pos_ul(*val);
 }
 int32_t PipetteModule::read_capactitance(int32_t *val) {
+  m_smtp2.pump_get_capacitance(val);
   return 0;
-  //  return m_smtp2.read_capacitance_val(*val);
 }
 int32_t PipetteModule::read_tip_state(int32_t *val) {
   *val = 0;
+  m_smtp2.pump_get_tip_state(val);
   return 0;
-  // bool    tipstate = false;
-  // int32_t ecode    = m_smtp2.read_tip_state(tipstate);
-  // *val             = tipstate ? 1 : 0;
-  // return ecode;
-}
-int32_t PipetteModule::_read_error_status() {
-  return 0;
-  // bool         isbusy    = false;
-  // err::error_t errorcode = err::ksucc;
-  // int32_t      retecode  = m_smtp2.getState(isbusy, errorcode);
-  // if (retecode != 0) {
-  //   return retecode;
-  // }
-  // if (errorcode != err::ksucc && errorcode != err::kpipette_error_NoError) {
-  //   return errorcode;
-  // }
-
-  // return 0;
 }

components/pipette_module/pipette_ctrl_module_v2.hpp

@@ -164,6 +164,7 @@ class PipetteModule : public ZIModule, public ZIPipetteCtrlModule {
 
   int32_t do_pipette_aspirate(int32_t ul,  int32_t zmotor_v);
   int32_t do_pipette_distribut(int32_t ul,  int32_t zmotor_v);
+  int32_t do_pipette_shake_up(int32_t ul, int32_t zmotor_v, int32_t times);
 
  private:
   int32_t befor_run();
@@ -176,7 +177,6 @@ class PipetteModule : public ZIModule, public ZIPipetteCtrlModule {
   int32_t read_tip_state(int32_t *val);
 
  private:
-  int32_t _read_error_status();
 
  private:
   void push_snesor_sample_data(int32_t motor_pos, int32_t cap_val, int32_t pressure_val);