修复归零失败BUG

components/iflytop_can_slave_modules/device_base_control_service.cpp

@@ -6,8 +6,9 @@ void DeviceBaseControlService::initialize(IflytopCanProtocolStackProcesser* prot
   m_protocol_processer = protocol_processer;
   m_deviceId           = deviceId;
   ZASSERT(m_protocol_processer != NULL);
-  activeRegs();
+
   m_slave = m_protocol_processer->createICPSSlaveModule("DeviceBaseControlService", this, 0);
+  activeRegs();
 }
 void DeviceBaseControlService::activeRegs() {
   m_protocol_processer->activeReg(m_slave, REG_DEVICE_ID, icps::kr, m_deviceId);
@@ -35,4 +36,4 @@ icps::error_t DeviceBaseControlService::onHostRegisterWriteEvent(IflytopCanProto
 }
 
 bool DeviceBaseControlService::isInEngineerMode() { return m_protocol_processer->readRegValue(REG_DEVICE_ENGINEER_MODE_MODE) != 0; }
-#endif
+#endif

components/iflytop_can_slave_v1/iflytop_can_slave.cpp

@@ -114,7 +114,8 @@ void IflytopCanProtocolStackProcesser::initialize(cfg_t *config) {
 }
 ICPSSlaveModule *IflytopCanProtocolStackProcesser::createICPSSlaveModule(const char *modulename, ICPSListener *listener, int regaddoff) {  //
   ZASSERT(m_listenerSize < ZARRAY_SIZE(m_listeners));
-  ICPSSlaveModule *container  = new ICPSSlaveModule();
+  ICPSSlaveModule *container = new ICPSSlaveModule();
+  ZASSERT(container != NULL);
   container->modulename       = modulename;
   container->listener         = listener;
   container->moduleRegAddoff  = regaddoff;

components/single_axis_motor_control/single_axis_motor_control.cpp

@@ -19,6 +19,9 @@ void SingleAxisMotorControler::initialize(const char* name, IflytopCanProtocolSt
   ZASSERT(motor);
   if (m_homegpio) {
     ZASSERT(m_homegpio->isItRisingAndItFallingEXITGPIO());
+    m_homegpio->regListener([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) {
+      if (m_gpioirqprocesser) m_gpioirqprocesser(GPIO_Pin, irqevent);
+    });
   }
 
   m_slave = m_processer->createICPSSlaveModule(name, this, m_regaddoff);
@@ -47,6 +50,9 @@ void SingleAxisMotorControler::initialize(const char* name, IflytopCanProtocolSt
   cfg_min_pos                      = m_processer->activeReg(m_slave, SAMC_REG_CFG_MIN_POS, icps::kwr, 0);                                     // 最小位置
   cfg_max_pos                      = m_processer->activeReg(m_slave, SAMC_REG_CFG_MAX_POS, icps::kwr, 0);                                     // 最大位置
   cfg_runtohome_keep_move_distance = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_KEEP_MOVE_DISTANCE, icps::kwr, 0);  // 归零阶段1移动距离
+
+  m_step_scheduler.initialize(10, 1);
+  ZHALCORE::getInstance()->regPeriodJob([this](ZHALCORE::Context& context) { this->periodJob(&context); }, 1);
 }
 
 icps::error_t SingleAxisMotorControler::onHostRegisterWriteEvent(icps::WriteEvent* event) {
@@ -171,6 +177,7 @@ icps::error_t SingleAxisMotorControler::runToHome() {
    *
    */
   m_step_scheduler.pushActionAction([this](StepActionContext* context) {  //
+    ZLOGI(m_name, "runToHome step 2 move away from zero point");
     regGpioIrqProcesser(nullptr);
     motor_moveBy(cfg_runtohome_leave_zero_point_distance->getValue(),  //
                  cfg_runhome_velocity->getValue(),                     //
@@ -183,6 +190,7 @@ icps::error_t SingleAxisMotorControler::runToHome() {
    * @brief STEP 3 返回原点
    */
   m_step_scheduler.pushActionAction([this](StepActionContext* context) {  //
+    ZLOGI(m_name, "runToHome step 3 move to zero point");
     regGpioIrqProcesser([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) {
       if (GPIO_Pin != m_homegpio) return;
       m_runToHomeContext.stopByIrq    = true;
@@ -217,6 +225,7 @@ icps::error_t SingleAxisMotorControler::runToHome() {
    * @brief STEP 4 移动到零点
    */
   m_step_scheduler.pushActionAction([this](StepActionContext* context) {  //
+    ZLOGI(m_name, "runToHome step 4 move to zero point");
     regGpioIrqProcesser(nullptr);
     motor_moveTo(0,                                 //
                  cfg_runhome_velocity->getValue(),  //
@@ -228,8 +237,12 @@ icps::error_t SingleAxisMotorControler::runToHome() {
   /**
    * @brief 异常处理
    */
-  m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); });
+  m_step_scheduler.regDoneCallback([this]() {
+    regGpioIrqProcesser(nullptr);
+    changeToState(kstate_idle);
+  });
   m_step_scheduler.regBreakCallback([this]() {
+    regGpioIrqProcesser(nullptr);
     m_exceptionReg->setValue(kexcep_loseZeroPoint);
     changeToState(kstate_exception);
   });
@@ -251,11 +264,19 @@ icps::error_t SingleAxisMotorControler::clearException() {
 }
 
 void SingleAxisMotorControler::changeToState(state_t state) {
+  ZLOGI(m_name, "changeState %d->%d", m_state, state);
   m_state = state;
   m_statusReg->setValue(m_state);
 }
 
-void SingleAxisMotorControler::periodJob(ZHALCORE::Context* context) {}
+void SingleAxisMotorControler::periodJob(ZHALCORE::Context* context) {
+  if (context->getScheduleTimes() % 33 == 0 /*33 ms*/) {
+    m_currentVelocityReg->setValue(m_motor->getVACTUAL());
+    m_currentPosReg->setValue(m_motor->getXACTUAL());
+    // ZLOGI(m_name, "%d", m_statusReg->getValue());
+  }
+  // m_statusReg->setValue(context->getScheduleTimes() % 4);
+}
 void SingleAxisMotorControler::motor_moveTo(int32_t pos, int32_t velocity, int32_t acc, int32_t dec) {
   ZLOGI(m_name, "motor_moveTo %d %d %d %d", pos, velocity, acc, dec);
   int32_t position = m_motor->getXACTUAL();

components/single_axis_motor_control/single_axis_motor_control.hpp

@@ -133,8 +133,14 @@ class SingleAxisMotorControler : public ICPSListener {
 
  private:
   bool motorIsStop(StepActionContext* context);
-  void regGpioIrqProcesser(ZGPIO::onirq_t irqprocesser) { m_gpioirqprocesser = irqprocesser; }
-  void clearGpioIrqProcesser() { m_gpioirqprocesser = NULL; }
+  void regGpioIrqProcesser(ZGPIO::onirq_t irqprocesser) {
+    CriticalContext cc;
+    m_gpioirqprocesser = irqprocesser;
+  }
+  void clearGpioIrqProcesser() {
+    CriticalContext cc;
+    m_gpioirqprocesser = NULL;
+  }
 };
 
 }  // namespace iflytop

components/tmc/basic/tmc_ic_interface.hpp

@@ -58,6 +58,7 @@ class IStepperMotor {
   virtual void setDeceleration(float accelerationpps2) = 0;  // 设置最大减速度
 
   virtual bool isReachTarget() = 0;
+  virtual bool isStoped()      = 0;
 };
 
 }  // namespace iflytop

components/tmc/ic/ztmc4361A.cpp

@@ -110,7 +110,7 @@ void TMC4361A::readWriteCover(uint8_t *data, size_t length) {
 TMC4361A::TMC4361A(/* args */) {
   m_driver_ic_type          = IC_TMC2130;
   m_lastCallPeriodicJobTick = 0;
-  m_reachtarget             = false;
+  // m_reachtarget             = false;
 }
 /**
  * @brief
@@ -251,18 +251,18 @@ int32_t tmc4361A_discardVelocityDecimals(int32_t value) {
   return value << 8;
 }
 void TMC4361A::rotate(int32_t velocity) {
+  m_motor_mode = kvelmode;
   PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0);
   writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity));
 }
 void TMC4361A::stop() { rotate(0); }
 void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) {
-  m_reachtarget = false;
+  m_motor_mode = kposmode;
   PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 1);
   writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocityMax));
   writeInt(TMC4361A_X_TARGET, position);
 }
 void TMC4361A::moveBy(int32_t relativePosition, uint32_t velocityMax) {
-  m_reachtarget = false;
   relativePosition += readInt(TMC4361A_XACTUAL);
   moveTo(relativePosition, velocityMax);
 }
@@ -289,10 +289,10 @@ uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) {
 }
 bool TMC4361A::isReachTarget() {
   uint32_t value = readInt(TMC4361A_STATUS);
-  if ((value & TMC4361A_TARGET_REACHED_F_MASK) > 0) {
-    return true;
+  if (m_motor_mode == kposmode) {
+    return (value & TMC4361A_TARGET_REACHED_F_MASK) > 0;
   } else {
-    return false;
+    return (value & TMC4361A_VEL_REACHED_F_MASK) && (readInt(TMC4361A_VMAX) == 0);
   }
 }
 /*******************************************************************************

components/tmc/ic/ztmc4361A.hpp

@@ -35,6 +35,11 @@ class TMC4361A : public IStepperMotor {
     ZGPIO *driverIC_resetPin;
   } cfg_t;
 
+  typedef enum {
+    kvelmode,
+    kposmode,
+  } motor_mode_t;
+
  private:
   int32_t        shadowRegister[TMC_REGISTER_COUNT];
   const uint8_t *m_registerAccessTable;
@@ -54,7 +59,7 @@ class TMC4361A : public IStepperMotor {
   ZGPIO *m_driverIC_ennPin;
   ZGPIO *m_driverIC_resetPin;
 
-  bool m_reachtarget;
+  motor_mode_t m_motor_mode = kvelmode;
 
  public:
   TMC4361A(/* args */);
@@ -122,6 +127,7 @@ class TMC4361A : public IStepperMotor {
    *******************************************************/
   int32_t      readSubICVersion();
   virtual bool isReachTarget();
+  virtual bool isStoped() { return isReachTarget(); }
 
  public:
   // only used in tmc4361A.cpp

components/tmc/ic/ztmc5130.cpp

@@ -131,12 +131,19 @@ uint32_t TMC5130::haspassedms(uint32_t now, uint32_t last) {
   }
 }
 bool TMC5130::isReachTarget() {
-  uint32_t        state = getTMC5130_RAMPSTAT();
-  Tmc5130RampStat event = Tmc5130RampStat(state);
-  if (event.isSetted(Tmc5130RampStat::ktmc5130_rs_posreached)) {
-    return true;
+  /**
+   * @brief
+   */
+  int mode = readInt(TMC5130_RAMPMODE);
+  if (mode == TMC5130_MODE_POSITION) {
+    uint32_t        state = getTMC5130_RAMPSTAT();
+    Tmc5130RampStat event = Tmc5130RampStat(state);
+    return event.isSetted(Tmc5130RampStat::ktmc5130_rs_posreached);
+  } else {
+    uint32_t        state = getTMC5130_RAMPSTAT();
+    Tmc5130RampStat event = Tmc5130RampStat(state);
+    return event.isSetted(Tmc5130RampStat::ktmc5130_rs_vzero) && event.isSetted(Tmc5130RampStat::ktmc5130_rs_velreached);
   }
-  return false;
 }
 /*******************************************************************************
  *                                    basic                                    *

components/tmc/ic/ztmc5130.hpp

@@ -124,6 +124,7 @@ class TMC5130 : public IStepperMotor {
 
   virtual void stop();
   virtual bool isReachTarget();  // 是否到达目标位置
+  virtual bool isStoped() { return isReachTarget(); }
 
   /*******************************************************************************
    *                             驱动器寄存器读写方法                              *