diff --git a/Auxiliary_utils/LibSidServo/PID_test/moving_model.files b/Auxiliary_utils/LibSidServo/PID_test/moving_model.files
index e522e0e..0086d29 100644
--- a/Auxiliary_utils/LibSidServo/PID_test/moving_model.files
+++ b/Auxiliary_utils/LibSidServo/PID_test/moving_model.files
@@ -1,5 +1,7 @@
 Dramp.c
 Dramp.h
+PID.c
+PID.h
 Sramp.c
 Sramp.h
 Tramp.c
diff --git a/Auxiliary_utils/LibSidServo/examples/goto.c b/Auxiliary_utils/LibSidServo/examples/goto.c
index 15c0796..a9a68e1 100644
--- a/Auxiliary_utils/LibSidServo/examples/goto.c
+++ b/Auxiliary_utils/LibSidServo/examples/goto.c
@@ -91,6 +91,7 @@ int main(int _U_ argc, char _U_ **argv){
     if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
     coordval_pair_t M, E;
     if(!getPos(&M, &E)) ERRX("Can't get current position");
+    printf("Current time: %.10f\n", Mount.currentT());
     DBG("xt: %g, x: %g", M.X.t, M.X.val);
     if(G.coordsoutput){
         if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n");
diff --git a/Auxiliary_utils/LibSidServo/main.c b/Auxiliary_utils/LibSidServo/main.c
index d6d267c..5fe1b9d 100644
--- a/Auxiliary_utils/LibSidServo/main.c
+++ b/Auxiliary_utils/LibSidServo/main.c
@@ -51,18 +51,16 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd);
  * @return time in seconds
  */
 double nanotime(){
-    //static struct timespec *start = NULL;
+    static double t0 = -1.;
     struct timespec now;
-    /*if(!start){
-        start = malloc(sizeof(struct timespec));
-        if(!start) return -1.;
-        if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.;
-    }*/
     if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.;
-    /*double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
-    double sd = (double)now.tv_sec - (double)start->tv_sec;
-    return sd + nd;*/
-    return (double)now.tv_sec + (double)now.tv_nsec * 1e-9;
+    if(t0 < 0.){
+        struct timespec start;
+        if(clock_gettime(CLOCK_REALTIME, &start)) return -1.;
+        t0 = (double)start.tv_sec + (double)start.tv_nsec * 1e-9
+             - (double)now.tv_sec + (double)now.tv_nsec * 1e-9;
+    }
+    return (double)now.tv_sec + (double)now.tv_nsec * 1e-9 + t0;
 }
 
 /**