...

LibSidServo/examples/dumpmoving_scmd.c

@@ -72,6 +72,8 @@ static conf_t Config = {
     .SepEncoder = 0
 };
 
+static coords_t M;
+
 // dump thread
 static void *dumping(void _U_ *u){
     dumpmoving(fcoords, 3600., G.Ncycles);
@@ -108,20 +110,22 @@ static int Wait(double tag){
     return TRUE;
 }
 
-// move X to 40 degr with given speed until given coord
+// move X/Y to 40 degr with given speed until given coord
 static void move(double target, double limit, double speed){
 #define SCMD()   do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0)
     green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed);
     short_command_t cmd = {0};
     if(*G.axis == 'X'){
-        cmd.Xmot = DEG2RAD(target);
+        cmd.Xmot = DEG2RAD(target) + M.X;
         cmd.Xspeed = DEG2RAD(speed);
+        limit = DEG2RAD(limit) + M.X;
     }else{
-        cmd.Ymot = DEG2RAD(target);
+        cmd.Ymot = DEG2RAD(target) + M.Y;
         cmd.Yspeed = DEG2RAD(speed);
+        limit = DEG2RAD(limit) + M.Y;
     }
     SCMD();
-    if(!Wait(DEG2RAD(limit))) signals(9);
+    if(!Wait(limit)) signals(9);
 #undef SCMD
 }
 
@@ -139,11 +143,11 @@ int main(int argc, char **argv){
             ERRX("Can't open %s", G.coordsoutput);
     }else fcoords = stdout;
     Config.MountReqInterval = G.reqint;
-    mcc_errcodes_t e = Mount.init(&Config);
-    if(e != MCC_E_OK){
+    if(MCC_E_OK != Mount.init(&Config)){
         WARNX("Can't init devices");
         return 1;
     }
+    if(!getPos(&M, NULL)) ERRX("Can't get current position");
     signal(SIGTERM, signals); // kill (-15) - quit
     signal(SIGHUP, SIG_IGN);  // hup - ignore
     signal(SIGINT, signals);  // ctrl+C - quit
@@ -151,7 +155,6 @@ int main(int argc, char **argv){
     signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
     // move to X=40 degr with different speeds
     pthread_t dthr;
-    chk0(G.Ncycles);
     logmnt(fcoords, NULL);
     if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
     // goto 1 degr with 1'/s
@@ -165,7 +168,7 @@ int main(int argc, char **argv){
     // and go back with 5deg/s
     move(0., 0., 5.);
     // be sure to move @ 0,0
-    Mount.moveTo(0., 0.);
+    Mount.moveTo(&M.X, &M.Y);
     // wait moving ends
     pthread_join(dthr, NULL);
 #undef SCMD