修改tmc51x0停止逻辑

tmcdriver/tmc51x0/tmc51x0.cpp

@@ -30,7 +30,7 @@ static const uint8_t tmc5130_defaultRegisterAccess[TMC5130_REGISTER_COUNT] = {
 #endif
 
 /***********************************************************************************************************************
- *                                                      撌亙�摰?                                                         *
+ *                                                      撌亙�嚙�?                                                         *
  ***********************************************************************************************************************/
 
 #define FIELD_SET(data, mask, shift, value) (data = (((data) & (~(mask))) | (((value) << (shift)) & (mask))))
@@ -107,7 +107,7 @@ void TMC51X0::writeField(uint8_t add, uint32_t mask, uint32_t shift, uint32_t va
 int32_t TMC51X0::readInt(uint8_t address) {
   /**
    * @WARNING:
-   *    餈䠷�瘝⊥��斗鱏撖���冽糓�血虾霂鳴��劐�撖���冽糓銝滚虾霂餅�隞伐���隞亙虾�質?餃��亦��啣�澆��坔��啣�潭糓銝滨㮾蝑厩�
+   *    餈䠷�瘝⊥��斗鱏撖���冽糓�血虾霂鳴��劐�撖���冽糓銝滚虾霂餅�隞伐���隞亙虾�踝蕭?嚙賢��亦��啣�澆��坔��啣�潭糓銝滨㮾蝑厩�
    */
   address = TMC51x0_ADDRESS(address);
 
@@ -176,8 +176,17 @@ int32_t TMC51X0::to_user_vel(int32_t vel) {  //
  ***********************************************************************************************************************/
 void TMC51X0::stop() {
   zlock_guard lkg(m_mutex);
-
+  int         mode = readInt(TMC5130_RAMPMODE);
+  if (mode == TMC5130_MODE_POSITION) {
+    // writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
+    writeInt(TMC5130_VMAX, 0);
+    while (getVACTUAL() > 5) {
+      osDelay(2);
+    }
+    writeInt(TMC5130_XTARGET, readInt(TMC5130_XACTUAL));
+  } else {
     rotate(0);
+  }
 }
 void TMC51X0::rotate(int32_t velocity) {
   zlock_guard lkg(m_mutex);