diff --git a/tmcdriver/tmc51x0/tmc51x0.cpp b/tmcdriver/tmc51x0/tmc51x0.cpp
index 5dc0ba7..9193dcc 100644
--- a/tmcdriver/tmc51x0/tmc51x0.cpp
+++ b/tmcdriver/tmc51x0/tmc51x0.cpp
@@ -208,10 +208,12 @@ void TMC51X0::stop() {
   int         mode = readInt(TMC5130_RAMPMODE);
   if (mode == TMC5130_MODE_POSITION) {
     // writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
-    writeInt(TMC5130_VMAX, 0);
-    while (getVACTUAL() > 5) {
+    rotate(0);
+    while (getVACTUAL() > 0) {
       osDelay(2);
     }
+    writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
+    writeInt(TMC5130_VMAX, 1);
     writeInt(TMC5130_XTARGET, readInt(TMC5130_XACTUAL));
   } else {
     rotate(0);