diff --git a/src/src/extras/StepperControl.cpp b/src/src/extras/StepperControl.cpp
index a491e6e..6992c16 100644
--- a/src/src/extras/StepperControl.cpp
+++ b/src/src/extras/StepperControl.cpp
@@ -61,10 +61,9 @@ void StepperControl::move(int nSteps, uint32_t msDelay, endAction_t endAction){
     return;
   }
   
-  upLink_t upLinkData = { .nSteps=nSteps, .msDelay=msDelay, .absoluteStep=false, endAction=endAction };
-  xQueueReceive(downLinkQueue,&downLinkData,0);
-  downLinkData.stepsRemaining=nSteps;
+  upLink_t upLinkData = { .nSteps=nSteps, .msDelay=msDelay, .action=MOVE, endAction=endAction };
   xQueueOverwrite(upLinkQueue,&upLinkData);
+  waitForAck();
 }
 
 //////////////////////////
@@ -75,10 +74,9 @@ void StepperControl::moveTo(int nPosition, uint32_t msDelay, endAction_t endActi
     return;
   }
   
-  upLink_t upLinkData = { .nSteps=nPosition, .msDelay=msDelay, .absoluteStep=true, .endAction=endAction };
-  xQueueReceive(downLinkQueue,&downLinkData,0);
-  downLinkData.stepsRemaining=nPosition-downLinkData.position;
+  upLink_t upLinkData = { .nSteps=nPosition, .msDelay=msDelay, .action=MOVETO, .endAction=endAction };
   xQueueOverwrite(upLinkQueue,&upLinkData);
+  waitForAck();
 }
 
 //////////////////////////
@@ -99,15 +97,24 @@ int StepperControl::position(){
 
 void StepperControl::setPosition(int pos){
   if(!stepsRemaining()){
-    downLinkData.position=pos;
-    xQueueOverwrite(downLinkQueue,&downLinkData);
+    upLink_t upLinkData = { .nSteps=pos, .msDelay=10, .action=SET_POSITION, .endAction=NONE };
+    xQueueOverwrite(upLinkQueue,&upLinkData);
+    waitForAck();
   } else {
-    ESP_LOGE(STEPPER_TAG,"can't set Position while motor is running");
+    ESP_LOGE(STEPPER_TAG,"can't set position while motor is running");
   }
 }
 
 //////////////////////////
 
+void StepperControl::waitForAck(){
+  downLinkData.ack=false;
+  while(downLinkData.ack==false)
+    xQueueReceive(downLinkQueue,&downLinkData,0);  
+};
+
+//////////////////////////
+
 void StepperControl::brake(){
   move(0,10,BRAKE);
   while(stepsRemaining());
@@ -130,18 +137,27 @@ void StepperControl::enable(){
 
 void StepperControl::motorTask(void *args){
   StepperControl *motor = (StepperControl *)args;
-  upLink_t upLinkData = { .nSteps=0, .msDelay=10, .absoluteStep=false, .endAction=NONE };
-  downLink_t downLinkData = { .stepsRemaining=0, .position=0 };  
+  upLink_t upLinkData;
+  downLink_t downLinkData;
   boolean running=false;
 
   for(;;){
     
     if(xQueueReceive(motor->upLinkQueue, &upLinkData, 0)){
-      if(upLinkData.absoluteStep)
-        upLinkData.nSteps-=downLinkData.position;
-      downLinkData.stepsRemaining=upLinkData.nSteps;
-      motor->onEnable();
+      switch(upLinkData.action){
+        case SET_POSITION:
+          downLinkData.position=upLinkData.nSteps;
+        break;
+        case MOVETO:
+          upLinkData.nSteps-=downLinkData.position;
+          [[fallthrough]];
+        case MOVE:
+          downLinkData.stepsRemaining=upLinkData.nSteps;
+          motor->onEnable();
+        break;
+      }
       running=true;
+      downLinkData.ack=true;
     }
 
     uint32_t msDelay=upLinkData.msDelay;
@@ -175,6 +191,7 @@ void StepperControl::motorTask(void *args){
     xQueueOverwrite(motor->downLinkQueue,&downLinkData);
     downLinkData.stepsRemaining+=dStep;
     downLinkData.position-=dStep;
+    downLinkData.ack=false;
     vTaskDelay(msDelay);  
   }
 }
diff --git a/src/src/extras/StepperControl.h b/src/src/extras/StepperControl.h
index 12c5581..b4b4f9e 100644
--- a/src/src/extras/StepperControl.h
+++ b/src/src/extras/StepperControl.h
@@ -51,28 +51,36 @@ class StepperControl {
     BRAKE
   };
 
+  enum action_t {
+    MOVE,
+    MOVETO,
+    SET_POSITION
+  };
+
   private:
   
   struct upLink_t {
     int nSteps;
     uint32_t msDelay;
-    boolean absoluteStep;
+    action_t action;
     endAction_t endAction;
   };
 
   struct downLink_t {
-    int stepsRemaining;
-    int position;
+    int stepsRemaining=0;
+    int position=0;
+    boolean ack=false;
   };
   
   float accelSteps=1;
   float accelSize=0;
-  downLink_t downLinkData = { .stepsRemaining=0, .position=0 };
+  downLink_t downLinkData;
 
   TaskHandle_t motorTaskHandle;
   QueueHandle_t upLinkQueue;
   QueueHandle_t downLinkQueue;
 
+  void waitForAck();
   virtual void onStep(boolean direction)=0;
   virtual void onEnable(){};
   virtual void onDisable(){};
diff --git a/src/src/extras/extras.ino b/src/src/extras/extras.ino
index e72f5a8..d371102 100644
--- a/src/src/extras/extras.ino
+++ b/src/src/extras/extras.ino
@@ -45,14 +45,14 @@ void setup() {
   motor=new Stepper_TB6612(23,32,22,14);
 
   motor->setStepType(StepperControl::HALF_STEP);
-  motor->setAccel(10,100);
+  motor->setAccel(10,10);
+  motor->setPosition(200);
   motor->move(100,2);
-  motor->setPosition(200);
   while(motor->stepsRemaining());
-  motor->setPosition(200);
-  delay(10);
-  Serial.printf("Position=%d\n",motor->position());
+  motor->setPosition(-200);
   motor->moveTo(0,2,StepperControl::BRAKE);
+  while(motor->position()<-11);
+  motor->disable();
 }
 
 //////////////////////////////////////