diff --git a/LibSidServo/PID.c b/LibSidServo/PID.c
index 8c8cd7d..8afe031 100644
--- a/LibSidServo/PID.c
+++ b/LibSidServo/PID.c
@@ -54,7 +54,7 @@ void pid_delete(PIDController_t **pid){
 double pid_calculate(PIDController_t *pid, double error, double dt){
     // calculate flowing integral
     double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
-    DBG("oldi/new: %g, %g", oldi, newi);
+    //DBG("oldi/new: %g, %g", oldi, newi);
     pid->pidIarray[pid->curIidx++] = newi;
     if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
     pid->integral += newi - oldi;
@@ -71,68 +71,79 @@ typedef struct{
 } PIDpair_t;
 
 typedef struct{
-    axis_status_t *state;
+    axis_status_t state;
     coordval_t position;
     coordval_t speed;
 } axisdata_t;
 /**
- * @brief process - Process PID for given axe
+ * @brief process - Process PID for given axis
  * @param tagpos - given coordinate of target position
  * @param endpoint - endpoint for this coordinate
  * @param pid - pid itself
  * @return calculated new speed or -1 for max speed
  */
 static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
-    if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT) return axis->speed.val; // data is too old or wrong
+    if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT){
+        DBG("target time: %g, axis time: %g - too big! (%g)", tagpos->t, axis->position.t, MCC_PID_MAX_DT);
+        return axis->speed.val; // data is too old or wrong
+    }
     double error = tagpos->val - axis->position.val, fe = fabs(error);
     PIDController_t *pid = NULL;
-    switch(*axis->state){
+    switch(axis->state){
         case AXIS_SLEWING:
             if(fe < MCC_MAX_POINTING_ERR){
-                *axis->state = AXIS_POINTING;
-                DBG("--> Pointing\n");
+                axis->state = AXIS_POINTING;
+                DBG("--> Pointing");
                 pid = pidpair->PIDC;
             }else{
-                DBG("Slewing...\n");
+                DBG("Slewing...");
                 return -1.; // max speed for given axis
             }
             break;
         case AXIS_POINTING:
             if(fe < MCC_MAX_GUIDING_ERR){
-                *axis->state = AXIS_GUIDING;
-                DBG("--> Guiding\n");
+                axis->state = AXIS_GUIDING;
+                DBG("--> Guiding");
                 pid = pidpair->PIDV;
             }else if(fe > MCC_MAX_POINTING_ERR){
-                DBG("--> Slewing\n");
-                *axis->state = AXIS_SLEWING;
+                DBG("--> Slewing");
+                axis->state = AXIS_SLEWING;
                 return -1.;
             } else pid = pidpair->PIDC;
             break;
         case AXIS_GUIDING:
             pid = pidpair->PIDV;
             if(fe > MCC_MAX_GUIDING_ERR){
-                DBG("--> Pointing\n");
-                *axis->state = AXIS_POINTING;
+                DBG("--> Pointing");
+                axis->state = AXIS_POINTING;
                 pid = pidpair->PIDC;
             }else if(fe < MCC_MAX_ATTARGET_ERR){
-                DBG("At target\n");
+                DBG("At target");
                 // TODO: we can point somehow that we are at target or introduce new axis state
-            }else DBG("Current error: %g\n", fe);
+            }else DBG("Current error: %g", fe);
             break;
-        case AXIS_STOPPED:
+        case AXIS_STOPPED: // start pointing to target; will change speed next time
+            DBG("AXIS STOPPED!!!!");
+            axis->state = AXIS_SLEWING;
+            return -1.;
         case AXIS_ERROR:
+            DBG("Can't move from erroneous state");
             return 0.;
     }
     if(!pid){
         DBG("WTF? Where is a PID?");
         return axis->speed.val;
     }
-    if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT) pid_clear(pid);
+    if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){
+        DBG("time diff too big: clear PID");
+        pid_clear(pid);
+    }
     double dt = tagpos->t - pid->prevT;
     if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
     pid->prevT = tagpos->t;
+    //DBG("CALC PID (er=%g, dt=%g)", error, dt);
     double tagspeed = pid_calculate(pid, error, dt);
-    if(*axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // velocity-based
+    if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // velocity-based
     return tagspeed; // coordinate-based
 }
 
@@ -159,16 +170,24 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
     mountdata_t m;
     coordpair_t tagspeed;
     if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
-    axisdata_t axe;
-    axe.state = &m.Xstate;
-    axe.position = m.encXposition;
-    axe.speed = m.encXspeed;
-    tagspeed.X = getspeed(&target->X, &pidX, &axe);
+    axisdata_t axis;
+    DBG("state: %d/%d", m.Xstate, m.Ystate);
+    axis.state = m.Xstate;
+    axis.position = m.encXposition;
+    axis.speed = m.encXspeed;
+    tagspeed.X = getspeed(&target->X, &pidX, &axis);
     if(tagspeed.X < 0. || tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
-    axe.state = &m.Ystate;
-    axe.position = m.encYposition;
-    axe.speed = m.encYspeed;
-    tagspeed.Y = getspeed(&target->Y, &pidY, &axe);
+    axis_status_t xstate = axis.state;
+    axis.state = m.Ystate;
+    axis.position = m.encYposition;
+    axis.speed = m.encYspeed;
+    tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
     if(tagspeed.Y < 0. || tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
+    axis_status_t ystate = axis.state;
+    if(m.Xstate != xstate || m.Ystate != ystate){
+        DBG("State changed");
+        setStat(xstate, ystate);
+    }
+    DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y);
     return Mount.moveWspeed(endpoint, &tagspeed);
 }
diff --git a/LibSidServo/examples/SSIIconf.c b/LibSidServo/examples/SSIIconf.c
index ea5e4d3..d399116 100644
--- a/LibSidServo/examples/SSIIconf.c
+++ b/LibSidServo/examples/SSIIconf.c
@@ -51,10 +51,10 @@ static sl_option_t confopts[] = {
     end_option
 };
 
-static void dumpaxe(char axe, axe_config_t *c){
+static void dumpaxis(char axis, axis_config_t *c){
 #define STRUCTPAR(p)    (c)->p
-#define DUMP(par) do{printf("%c%s=%g\n", axe, #par, STRUCTPAR(par));}while(0)
-#define DUMPD(par) do{printf("%c%s=%g\n", axe, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
+#define DUMP(par) do{printf("%c%s=%g\n", axis, #par, STRUCTPAR(par));}while(0)
+#define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
     DUMPD(accel);
     DUMPD(backlash);
     DUMPD(errlimit);
@@ -99,12 +99,12 @@ static void dumpHWconf(){
 #define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
 #define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
 #define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
-    green("X axe configuration:\n");
-    dumpaxe('X', &HW.Xconf);
+    green("X axis configuration:\n");
+    dumpaxis('X', &HW.Xconf);
     green("X bits:\n");
     dumpxbits(&HW.xbits);
-    green("Y axe configuration:\n");
-    dumpaxe('Y', &HW.Yconf);
+    green("Y axis configuration:\n");
+    dumpaxis('Y', &HW.Yconf);
     green("Y bits:\n");
     dumpybits(&HW.ybits);
     green("Other:\n");
@@ -143,10 +143,12 @@ int main(int argc, char** argv){
     }
     if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
     if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
+    /*
     char *c = sl_print_opts(confopts, TRUE);
     green("Got configuration:\n");
     printf("%s\n", c);
     FREE(c);
+    */
     dumpHWconf();
     /*
     if(G.hwconffile && G.writeconf){
diff --git a/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c
index 89abb73..1271a21 100644
--- a/LibSidServo/examples/conf.c
+++ b/LibSidServo/examples/conf.c
@@ -31,14 +31,18 @@ static conf_t Config = {
     .EncoderReqInterval = 0.05,
     .SepEncoder = 2,
     .EncoderSpeedInterval = 0.1,
-    .XPIDC.P = 0.5,
-    .XPIDC.D = 0.05,
-    .XPIDV.P = 0.5,
-    .XPIDV.D = 0.05,
-    .YPIDC.P = 0.5,
-    .YPIDC.D = 0.05,
+    .XPIDC.P = 0.8,
+    .XPIDC.I = 0.1,
+    .XPIDC.D = 0.3,
+    .XPIDV.P = 1.,
+    .XPIDV.I = 0.01,
+    .XPIDV.D = 0.2,
+    .YPIDC.P = 0.8,
+    .YPIDC.I = 0.1,
+    .YPIDC.D = 0.3,
     .YPIDV.P = 0.5,
-    .YPIDV.D = 0.05,
+    .YPIDV.I = 0.2,
+    .YPIDV.D = 0.5,
 };
 
 static sl_option_t opts[] = {
diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c
index 62019aa..27fe123 100644
--- a/LibSidServo/examples/dump.c
+++ b/LibSidServo/examples/dump.c
@@ -127,7 +127,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
 }
 
 /**
- * @brief waitmoving - wait until moving by both axes stops at least for N cycles
+ * @brief waitmoving - wait until moving by both axiss stops at least for N cycles
  * @param N - amount of stopped cycles
  */
 void waitmoving(int N){
diff --git a/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c
index 621f6f5..10e73f5 100644
--- a/LibSidServo/examples/scmd_traectory.c
+++ b/LibSidServo/examples/scmd_traectory.c
@@ -38,11 +38,12 @@ typedef struct{
     double X0;          // starting point of traectory (-30..30 degr)
     double Y0;          // -//-
     char *coordsoutput; // dump file
+    char *errlog;       // log with position errors
     char *tfn;          // traectory function name
     char *conffile;
 } parameters;
 
-static FILE *fcoords = NULL;
+static FILE *fcoords = NULL, *errlog = NULL;
 static pthread_t dthr;
 static parameters G = {
     .Ncycles = 40,
@@ -61,12 +62,13 @@ static sl_option_t cmdlnopts[] = {
     {"coordsfile",  NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
     {"reqinterval", NEED_ARG,   NULL,   'i',    arg_double, APTR(&G.reqint),    "mount requests interval (default: 0.1 second)"},
     {"traectory",   NEED_ARG,   NULL,   't',    arg_string, APTR(&G.tfn),       "used traectory function (default: sincos)"},
-    {"xmax",        NEED_ARG,   NULL,   'X',    arg_double, APTR(&G.Xmax),      "maximal X coordinate for traectory (default: 45 degrees)"},
-    {"ymax",        NEED_ARG,   NULL,   'Y',    arg_double, APTR(&G.Ymax),      "maximal X coordinate for traectory (default: 45 degrees)"},
+    {"xmax",        NEED_ARG,   NULL,   'X',    arg_double, APTR(&G.Xmax),      "maximal abs X coordinate for traectory (default: 45 degrees)"},
+    {"ymax",        NEED_ARG,   NULL,   'Y',    arg_double, APTR(&G.Ymax),      "maximal abs Y coordinate for traectory (default: 45 degrees)"},
     {"tmax",        NEED_ARG,   NULL,   'T',    arg_double, APTR(&G.tmax),      "maximal duration time of emulation (default: 300 seconds)"},
     {"x0",          NEED_ARG,   NULL,   '0',    arg_double, APTR(&G.X0),        "starting X-coordinate of traectory (default: 10 degrees)"},
     {"y0",          NEED_ARG,   NULL,   '1',    arg_double, APTR(&G.Y0),        "starting Y-coordinate of traectory (default: 10 degrees)"},
     {"conffile",    NEED_ARG,   NULL,   'C',    arg_string, APTR(&G.conffile),  "configuration file name"},
+    {"errlog",      NEED_ARG,   NULL,   'e',    arg_string, APTR(&G.errlog),    "file with errors log"},
     end_option
 };
 
@@ -76,6 +78,8 @@ void signals(int sig){
         signal(sig, SIG_IGN);
         DBG("Get signal %d, quit.\n", sig);
     }
+    Mount.stop();
+    sleep(1);
     Mount.quit();
     if(fcoords) fclose(fcoords);
     exit(sig);
@@ -90,22 +94,39 @@ static void *dumping(void _U_ *u){
 static void runtraectory(traectory_fn tfn){
     if(!tfn) return;
     coordval_pair_t telXY;
-    coordpair_t traectXY;
-    double t0 = Mount.currentT();
-    double tlast = 0.;
+    coordval_pair_t target;
+    coordpair_t traectXY, endpoint;
+    endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
+    double t0 = Mount.currentT(), tlast = 0.;
+    double tlastX = 0., tlastY = 0.;
     while(1){
         if(!telpos(&telXY)){
             WARNX("No next telescope position");
             return;
         }
-        if(telXY.X.t == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind
-        tlast = (telXY.X.t + telXY.Y.t) / 2.;
+        if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind
+        DBG("\n\nTELPOS: %g'/%g' measured @ %g/%g", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), telXY.X.t, telXY.Y.t);
+        tlastX = telXY.X.t; tlastY = telXY.Y.t;
         double t = Mount.currentT();
-        if(telXY.X.val > G.Xmax || telXY.Y.val > G.Ymax || t - t0 > G.tmax) break;
+        if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break;
         if(!traectory_point(&traectXY, t)) break;
-        DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
+        target.X.val = traectXY.X; target.Y.val = traectXY.Y;
+        target.X.t = target.Y.t = t;
+        // check whether we should change direction
+        if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax;
+        else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax;
+        if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax;
+        else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax;
+        DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
+        DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
+        DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
+        if(errlog)
+            fprintf(errlog, "%10.4g  %10.4g  %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
+        if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!");
+        while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50);
+        tlast = t;
     }
-    WARNX("No next traectory point");
+    WARNX("No next traectory point or emulation ends");
 }
 
 int main(int argc, char **argv){
@@ -118,6 +139,12 @@ int main(int argc, char **argv){
     G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
     if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
         ERRX("X0 and Y0 should be -30..30 degrees");
+    if(G.errlog){
+        if(!(errlog = fopen(G.errlog, "w")))
+            ERRX("Can't open error log %s", G.errlog);
+        else
+            fprintf(errlog, "#    time      Xerr''      Yerr''   // target - real\n");
+    }
     if(G.coordsoutput){
         if(!(fcoords = fopen(G.coordsoutput, "w")))
             ERRX("Can't open %s", G.coordsoutput);
diff --git a/LibSidServo/examples/simpleconv.h b/LibSidServo/examples/simpleconv.h
index 8200def..099ff86 100644
--- a/LibSidServo/examples/simpleconv.h
+++ b/LibSidServo/examples/simpleconv.h
@@ -20,10 +20,10 @@
 
 #include <math.h>
 
-#define DEG2RAD(d)  (d/180.*M_PI)
-#define ASEC2RAD(d) (d/180.*M_PI/3600.)
-#define AMIN2RAD(d) (d/180.*M_PI/60.)
-#define RAD2DEG(r)  (r/M_PI*180.)
-#define RAD2ASEC(r) (r/M_PI*180.*3600.)
-#define RAD2AMIN(r) (r/M_PI*180.*60.)
+#define DEG2RAD(d)  ((d)/180.*M_PI)
+#define ASEC2RAD(d) ((d)/180.*M_PI/3600.)
+#define AMIN2RAD(d) ((d)/180.*M_PI/60.)
+#define RAD2DEG(r)  ((r)/M_PI*180.)
+#define RAD2ASEC(r) ((r)/M_PI*180.*3600.)
+#define RAD2AMIN(r) ((r)/M_PI*180.*60.)
 
diff --git a/LibSidServo/examples/traectories.c b/LibSidServo/examples/traectories.c
index 1977880..ebc31cf 100644
--- a/LibSidServo/examples/traectories.c
+++ b/LibSidServo/examples/traectories.c
@@ -30,10 +30,6 @@ static traectory_fn cur_traectory = NULL;
 // starting point of traectory
 static coordpair_t XYstart = {0};
 static double tstart = 0.;
-// convert Xe/Ye to approximate motor coordinates:
-// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have
-// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0
-static coordval_pair_t XYcor = {0};
 
 /**
  * @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
@@ -52,9 +48,6 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
         if(MCC_E_OK == Mount.getMountData(&mdata)) break;
     }
     if(ntries == 10) return FALSE;
-    XYcor.X.val = mdata.motXposition.val - mdata.encXposition.val;
-    XYcor.Y.val = mdata.motYposition.val - mdata.encYposition.val;
-    DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X.val), DEG2RAD(XYcor.Y.val));
     return TRUE;
 }
 
@@ -83,8 +76,8 @@ int telpos(coordval_pair_t *curpos){
     }
     if(ntries == 10) return FALSE;
     coordval_pair_t pt;
-    pt.X.val = XYcor.X.val + mdata.encXposition.val;
-    pt.Y.val = XYcor.Y.val + mdata.encYposition.val;
+    pt.X.val = mdata.encXposition.val;
+    pt.Y.val = mdata.encYposition.val;
     pt.X.t = mdata.encXposition.t;
     pt.Y.t = mdata.encYposition.t;
     if(curpos) *curpos = pt;
@@ -94,7 +87,7 @@ int telpos(coordval_pair_t *curpos){
 // X=X0+1'/s, Y=Y0+15''/s
 int Linear(coordpair_t *nextpt, double t){
     coordpair_t pt;
-    pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
+    pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
     pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
     if(nextpt) *nextpt = pt;
     return TRUE;
@@ -103,7 +96,7 @@ int Linear(coordpair_t *nextpt, double t){
 // X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
 int SinCos(coordpair_t *nextpt, double t){
     coordpair_t pt;
-    pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
+    pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
     pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
     if(nextpt) *nextpt = pt;
     return TRUE;
@@ -116,8 +109,8 @@ typedef struct{
 } tr_names;
 
 static tr_names names[] = {
-    {Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"},
-    {SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
+    {Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"},
+    {SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
     {NULL, NULL, NULL}
 };
 
diff --git a/LibSidServo/main.c b/LibSidServo/main.c
index b0050ea..6927a4b 100644
--- a/LibSidServo/main.c
+++ b/LibSidServo/main.c
@@ -86,7 +86,6 @@ void getModData(mountdata_t *mountdata){
     if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
     movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
     if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
-    bzero(mountdata, sizeof(mountdata_t));
     mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow;
     mountdata->motXposition.val = mountdata->encXposition.val  = Xp.coord;
     mountdata->motYposition.val  = mountdata->encYposition.val  = Yp.coord;
@@ -210,6 +209,22 @@ static int chkYs(double s){
     return TRUE;
 }
 
+// set SLEWING state if axis was stopped later
+static void setslewingstate(){
+    //FNAME();
+    mountdata_t d;
+    if(MCC_E_OK == getMD(&d)){
+        axis_status_t newx = d.Xstate, newy = d.Ystate;
+        //DBG("old state: %d/%d", d.Xstate, d.Ystate);
+        if(d.Xstate == AXIS_STOPPED) newx = AXIS_SLEWING;
+        if(d.Ystate == AXIS_STOPPED) newy = AXIS_SLEWING;
+        if(newx != d.Xstate || newy != d.Ystate){
+            DBG("Started moving -> slew");
+            setStat(newx, newy);
+        }
+    }else DBG("CAN't GET MOUNT DATA!");
+}
+
 /*
 static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
     (void)target;
@@ -241,7 +256,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){
     cmd.Yspeed = MCC_MAX_Y_SPEED;
     mcc_errcodes_t r = shortcmd(&cmd);
     if(r != MCC_E_OK) return r;
-    setStat(AXIS_SLEWING, AXIS_SLEWING);
+    setslewingstate();
     return MCC_E_OK;
 }
 
@@ -279,7 +294,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
     cmd.Yspeed = speed->Y;
     mcc_errcodes_t r = shortcmd(&cmd);
     if(r != MCC_E_OK) return r;
-    setStat(AXIS_SLEWING, AXIS_SLEWING);
+    setslewingstate();
     return MCC_E_OK;
 }
 
@@ -288,6 +303,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
  * @return errcode
  */
 static mcc_errcodes_t emstop(){
+    FNAME();
     if(Conf.RunModel){
         double curt = nanotime();
         Xmodel->emergency_stop(Xmodel, curt);
@@ -299,6 +315,7 @@ static mcc_errcodes_t emstop(){
 }
 // normal stop
 static mcc_errcodes_t stop(){
+    FNAME();
     if(Conf.RunModel){
         double curt = nanotime();
         Xmodel->stop(Xmodel, curt);
@@ -323,6 +340,7 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
         if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
         param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
         if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
+        setslewingstate();
         return MCC_E_OK;
     }
     SSscmd s = {0};
@@ -336,6 +354,7 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
     s.YBits = cmd->YBits;
     DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed);
     if(!cmdS(&s)) return MCC_E_FAILED;
+    setslewingstate();
     return MCC_E_OK;
 }
 
@@ -353,6 +372,7 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
         if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
         param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
         if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
+        setslewingstate();
         return MCC_E_OK;
     }
     SSlcmd l = {0};
@@ -365,6 +385,7 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
     l.Xatime = S2ADDER(cmd->Xatime);
     l.Yatime = S2ADDER(cmd->Yatime);
     if(!cmdL(&l)) return MCC_E_FAILED;
+    setslewingstate();
     return MCC_E_OK;
 }
 
diff --git a/LibSidServo/main.h b/LibSidServo/main.h
index e642f1a..0eb9fe2 100644
--- a/LibSidServo/main.h
+++ b/LibSidServo/main.h
@@ -69,7 +69,7 @@ double LS_calc_slope(less_square_t *l, double x, double t);
     #define COLOR_OLD           "\033[0;0;0m"
     #define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
     fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
-    #define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \
+    #define DBG(...) do{ fprintf(stderr, COLOR_RED "%s " COLOR_OLD, __func__); \
         fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
         fprintf(stderr, __VA_ARGS__);           \
         fprintf(stderr, "\n");} while(0)
diff --git a/LibSidServo/movingmodel.c b/LibSidServo/movingmodel.c
index c1c61e8..83936ac 100644
--- a/LibSidServo/movingmodel.c
+++ b/LibSidServo/movingmodel.c
@@ -37,7 +37,10 @@ static void chkminmax(double *min, double *max){
 movemodel_t *model_init(limits_t *l){
     if(!l) return FALSE;
     movemodel_t *m = calloc(1, sizeof(movemodel_t));
+    // we can't use memcpy or assign as Times/Params would be common for all
     *m = trapez;
+    m->Times = calloc(STAGE_AMOUNT, sizeof(double));
+    m->Params = calloc(STAGE_AMOUNT, sizeof(moveparam_t));
     moveparam_t *max = &l->max, *min = &l->min;
     if(min->speed < 0.) min->speed = -min->speed;
     if(max->speed < 0.) max->speed = -max->speed;
diff --git a/LibSidServo/movingmodel.h b/LibSidServo/movingmodel.h
index 36e93c3..142e138 100644
--- a/LibSidServo/movingmodel.h
+++ b/LibSidServo/movingmodel.h
@@ -60,8 +60,8 @@ typedef struct movemodel{
     moveparam_t Max;
     movingstage_t movingstage;
     movestate_t state;
-    double Times[STAGE_AMOUNT];
-    moveparam_t Params[STAGE_AMOUNT];
+    double *Times;
+    moveparam_t *Params;
     moveparam_t curparams;                    // init values of limits, jerk
     int (*calculate)(struct movemodel *m, moveparam_t *target, double t);        // calculate stages of traectory beginning from t
     movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t);  // calculate next model point for time t
diff --git a/LibSidServo/ramp.c b/LibSidServo/ramp.c
index 5859394..c0efe5e 100644
--- a/LibSidServo/ramp.c
+++ b/LibSidServo/ramp.c
@@ -23,7 +23,12 @@
 
 #include "main.h"
 #include "ramp.h"
-
+/*
+#ifdef EBUG
+#undef DBG
+#define DBG(...)
+#endif
+*/
 static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
 
 static void emstop(movemodel_t *m, double _U_ t){
@@ -66,9 +71,9 @@ ret:
  * @return FALSE if can't move with given parameters
  */
 static int calc(movemodel_t *m, moveparam_t *x, double t){
-    DBG("coord/speed: %g/%g", x->coord, x->speed);
     if(!x) return FALSE;
     pthread_mutex_lock(&m->mutex);
+    DBG("target coord/speed: %g/%g; current coord: %g", x->coord, x->speed, m->curparams.coord);
     int ret = FALSE;
     if(x->coord < m->Min.coord || x->coord > m->Max.coord){
         DBG("Wrong coordinage [%g, %g]", m->Min.coord, m->Max.coord);
@@ -101,6 +106,33 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
         ret = TRUE;
         goto ret;
     }
+
+#if 0
+model_move2 (/home/eddy/C-files/LibSidServo/movingmodel.c, line 63): MOVE to -0.785398 at speed 0.00408621
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 76): target coord/speed: -0.785398/0.00408621; current coord: 0.181616
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 91): Dx=0.967014, sign=-1, dt23=0.0185812, dx23=3.79634e-05
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 101): dt0s=0.0377057, dx0s=0.000156326, curspeed=0.0082919
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 110): SIGN of speed should be changed!
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 0: t=3.9028, coord=0.181616, speed=0.0082919, accel=-0.219911
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 1: t=3.9405, coord=0.181772, speed=0, accel=-0.219911
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 2: t=3.95908, coord=0.181734, speed=-0.00408621, accel=0.219911
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 3: t=3.97766, coord=-0.785398, speed=0, accel=0
+model_move2 (/home/eddy/C-files/LibSidServo/movingmodel.c, line 63): MOVE to -0.785398 at speed 0.0361538
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 76): target coord/speed: -0.785398/0.0361538; current coord: 0.177343
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 91): Dx=0.962741, sign=-1, dt23=0.218049, dx23=0.00394164
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 101): dt0s=0.185651, dx0s=0.00285737, curspeed=0.0307821
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 110): SIGN of speed should be changed!
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 0: t=3.9028, coord=0.177343, speed=0.0307821, accel=-0.165806
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 1: t=4.08845, coord=0.1802, speed=0, accel=-0.165806
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 2: t=4.3065, coord=0.176259, speed=-0.0361538, accel=0.165806
+calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 3: t=4.52455, coord=-0.785398, speed=0, accel=0
+proc (/home/eddy/C-files/LibSidServo/ramp.c, line 233): REACHED STOPping stage @ t=3.97837
+proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[3]=3.97766,
+proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[2]=3.95908,
+proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[1]=3.9405,
+proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[0]=3.9028,
+
+#endif
     if(m->curparams.speed * sign < 0. || m->state == ST_STOP || (dx0s > Dx + coord_tolerance)){ // we should change speed sign
         DBG("SIGN of speed should be changed!");
         double sign_current = (m->curparams.speed >= 0.) ? 1. : -1.;
@@ -225,6 +257,11 @@ static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
     if(m->movingstage == STAGE_STOPPED){
         m->curparams.coord = m->Params[STAGE_STOPPED].coord;
         pthread_mutex_unlock(&m->mutex);
+        DBG("REACHED STOPping stage @ t=%g", t);
+        for(int s = STAGE_STOPPED; s >= 0; --s){
+            DBG("T[%d]=%g, ", s, m->Times[s]);
+        }
+        fflush(stdout);
         emstop(m, t);
         goto ret;
     }
diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c
index 400213b..8e7f82b 100644
--- a/LibSidServo/serial.c
+++ b/LibSidServo/serial.c
@@ -243,7 +243,10 @@ static int getmntbyte(){
         if(FD_ISSET(mntfd, &rfds)){
             ssize_t l = read(mntfd, &byte, 1);
             //DBG("MNT read=%zd byte=0x%X", l, byte);
-            if(l != 1) return -2; // disconnected ??
+            if(l != 1){
+                DBG("Mount disconnected?");
+                return -2; // disconnected ??
+            }
             break;
         } else return -1;
     }while(1);
@@ -586,6 +589,7 @@ mcc_errcodes_t getMD(mountdata_t  *d){
 }
 
 void setStat(axis_status_t Xstate, axis_status_t Ystate){
+    DBG("set x/y state to %d/%d", Xstate, Ystate);
     pthread_mutex_lock(&datamutex);
     mountdata.Xstate = Xstate;
     mountdata.Ystate = Ystate;
@@ -594,7 +598,10 @@ void setStat(axis_status_t Xstate, axis_status_t Ystate){
 
 // write-read without locking mutex (to be used inside other functions)
 static int wr(const data_t *out, data_t *in, int needeol){
-    if((!out && !in) || mntfd < 0) return FALSE;
+    if((!out && !in) || mntfd < 0){
+        DBG("Wrong arguments or no mount fd");
+        return FALSE;
+    }
     clrmntbuf();
     //double t0 = nanotime();
     if(out){
diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h
index 9e9f468..e43bacf 100644
--- a/LibSidServo/sidservo.h
+++ b/LibSidServo/sidservo.h
@@ -34,7 +34,7 @@ extern "C"
 
 // acceptable position error - 0.1''
 #define MCC_POSITION_ERROR      (5e-7)
-// acceptable disagreement between motor and axe encoders - 2''
+// acceptable disagreement between motor and axis encoders - 2''
 #define MCC_ENCODERS_ERROR      (1e-7)
 
 // max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
@@ -56,10 +56,11 @@ extern "C"
 // normal PID refresh interval
 #define MCC_PID_REFRESH_DT      (0.1)
 // boundary conditions for axis state: "slewing/pointing/guiding"
-// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 12 degrees
-#define MCC_MAX_POINTING_ERR    (0.20943951)
-// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "slewing": 15''
-#define MCC_MAX_GUIDING_ERR     (7.272205e-5)
+// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 5 degrees
+//#define MCC_MAX_POINTING_ERR    (0.20943951)
+#define MCC_MAX_POINTING_ERR    (0.08726646)
+// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg
+#define MCC_MAX_GUIDING_ERR     (0.026179939)
 // if error less than this value we suppose that target is captured and guiding is good: 0.1''
 #define MCC_MAX_ATTARGET_ERR    (4.8481368e-7)
 
@@ -89,7 +90,7 @@ typedef struct{
     double  EncoderReqInterval;     // interval between subsequent encoder requests (seconds)
     double  EncoderSpeedInterval;   // interval between speed calculations
     int     RunModel;               // == 1 if you want to use model instead of real mount
-    PIDpar_t XPIDC;                 // gain parameters of PID for both axes (C - coordinate driven, V - velocity driven)
+    PIDpar_t XPIDC;                 // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
     PIDpar_t XPIDV;
     PIDpar_t YPIDC;
     PIDpar_t YPIDV;
@@ -193,7 +194,7 @@ typedef struct{
     double Yatime;      // 28
 } long_command_t; // long command
 
-// hardware axe configuration
+// hardware axis configuration
 typedef struct{
     double accel;       // Default Acceleration, rad/s^2
     double backlash;    // Backlash (???)
@@ -204,13 +205,13 @@ typedef struct{
     double outplimit;   // Output Limit, percent (0..100)
     double currlimit;   // Current Limit (A)
     double intlimit;    // Integral Limit (???)
-} __attribute__((packed)) axe_config_t;
+} __attribute__((packed)) axis_config_t;
 
 // hardware configuration
 typedef struct{
-    axe_config_t Xconf;
+    axis_config_t Xconf;
     xbits_t xbits;
-    axe_config_t Yconf;
+    axis_config_t Yconf;
     ybits_t ybits;
     uint8_t address;
     double eqrate;      // Equatorial Rate (???)
diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c
index 51c6faa..51ed8b1 100644
--- a/LibSidServo/ssii.c
+++ b/LibSidServo/ssii.c
@@ -36,19 +36,18 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
     return checksum;
 }
 
-
-static void axestat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
+// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
+static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
     if(*prev == INT32_MAX){
         *stat = AXIS_STOPPED;
         DBG("START");
     }else if(*stat != AXIS_STOPPED){
-        if(*prev == cur  && ++(*nstopped) > MOTOR_STOPPED_CNT){
+        if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
             *stat = AXIS_STOPPED;
-            DBG("AXE stopped");
+            DBG("AXIS stopped");
         }
     }else if(*prev != cur){
-        DBG("AXE moving");
-        //*stat = AXIS_SLEWING;
+        DBG("AXIS moving");
         *nstopped = 0;
     }
     *prev = cur;
@@ -57,8 +56,8 @@ static void axestat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *st
 static void ChkStopped(const SSstat *s, mountdata_t *m){
     static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
     static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
-    axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
-    axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
+    chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
+    chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
 }
 
 /**
@@ -191,14 +190,14 @@ mcc_errcodes_t updateMotorPos(){
             DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
             mcc_errcodes_t OK = MCC_E_OK;
             if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
-                DBG("NEED to sync X: motors=%g, axes=%g", md.motXposition.val, md.encXposition.val);
+                DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val);
                 if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
                     DBG("Xpos sync failed!");
                     OK = MCC_E_FAILED;
                 }else DBG("Xpos sync OK, Dt=%g", nanotime() - t0);
             }
             if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
-                DBG("NEED to sync Y: motors=%g, axes=%g", md.motYposition.val, md.encYposition.val);
+                DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val);
                 if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
                     DBG("Ypos sync failed!");
                     OK = MCC_E_FAILED;
diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h
index 8819c42..504bb1b 100644
--- a/LibSidServo/ssii.h
+++ b/LibSidServo/ssii.h
@@ -194,8 +194,8 @@
 #define X_ENC_SIGN          (-1.)
 #define Y_ENC_SIGN          (-1.)
 // encoder position to radians and back
-#define X_ENC2RAD(n)    ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
-#define Y_ENC2RAD(n)    ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
+#define X_ENC2RAD(n)    ang2half(X_ENC_SIGN * 2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
+#define Y_ENC2RAD(n)    ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)((n)-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
 #define X_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
 #define Y_RAD2ENC(r)    ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))