   Calibrate();
   Serial.println("Done.");
   
-  thisEncoder=AdaEncoder::genie(&clicks, &id);
+  pos = 0;
   
-  int oldPos = 0;
   while(true)
   {
-    thisEncoder=AdaEncoder::genie(&clicks, &id);
-    if (thisEncoder != NULL) {
-      pos += clicks;
-      if (clicks != 0)
-      {
-        Serial.println(pos);
-      }
-    }
-  }    
-    
-    /*
-    int i = digitalRead(Enc1);
-    int j = digitalRead(Enc2);
-    sprintf(buf, "%d %d", i, j);
-    Serial.println(buf);
-    */
-   
-   /* 
-    int newPos = encoder.read();
-    if (newPos != oldPos)
-    {
-      Serial.println(newPos);
-      oldPos = newPos;
-    }
-    */
+    GoTo(1000);
+    delay(500);
+    pos = myEncoder.read();
+    Serial.println(pos);
+
+    GoTo(8000);
+    delay(500);
+    pos = myEncoder.read();
+    Serial.println(pos);
   }
 }
+
+void GoTo(long pos)
+{
+  long myPos;
+  do 
+  {
+    myPos = myEncoder.read();
+    if (abs(myPos - pos) < EPSILON)
+    {
+      SetMotorSpeed(0);
+    }
+    else if (myPos < pos)
+    {
+      SetMotorSpeed(255);
+    }
+    else
+    {
+      SetMotorSpeed(-255);
+    }
+  }
+  while (abs(myPos - pos) > EPSILON);
+
+  SetMotorSpeed(0);
+  sprintf(buf, "%ld %ld", pos, myPos);
+  Serial.println(buf);
+}  
+
 
 void Calibrate()
 {

  {
    ;
  }
  encoder.write(0);
  }
  SetMotorSpeed(0);
}
  
 
void SetMotorSpeed(signed int motorSpeed)

    pinMode(E1, OUTPUT);   
    pinMode(E2, OUTPUT);   
    pinMode(ST, INPUT);   
    pinMode(ENC1, INPUT);   
    pinMode(ENC2, INPUT);   
}

void loop()
{
  Serial.println("Hallo Welt!.");
  delay(3000);
  Serial.println("Calibrating...");
