...

LibSidServo/examples/Readme.md

@@ -0,0 +1,24 @@
+Some examples of usage of libsidservo
+=====================================
+
+## Auxiliary files
+
+*dump.c*, *dump.h* - base logging and dumping functions, also some useful functions like get current position and move to zero if current position isn't at zero.
+
+*traectories.c*, *traectories.h* - modeling simple moving object traectories; also some functions like get current position in encoders' angles setting to zero at motors' zero.
+
+*simpleconv.h*
+
+
+## Examples
+
+*dumpmoving.c* (`dump`) - dump moving relative starting point by simplest text commands "X" and "Y".
+
+*dumpmoving_scmd.c* (`dump_s`) - moving relative starting point using "short" binary command.
+
+*dumpswing.c* (`dumpswing`) - shake telescope around starting point by one of axis.
+
+*goto.c* (`goto`) - get current coordinates or go to given (by simplest "X/Y" commands).
+
+*scmd_traectory.c* (`traectory_s`) - try to move around given traectory using "short" binary commands.
+

LibSidServo/examples/dump.c

@@ -107,13 +107,13 @@ void waitmoving(int N){
 }
 
 /**
- * @brief getMotPos - get current
+ * @brief getPos - get current position
  * @param mot (o) - motor position (or NULL)
  * @param Y (o) - encoder position (or NULL)
  * @return FALSE if failed
  */
 int getPos(coords_t *mot, coords_t *enc){
-    mountdata_t mdata;
+    mountdata_t mdata = {0};
     int errcnt = 0;
     do{
         if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
@@ -137,7 +137,8 @@ void chk0(int ncycles){
     if(!getPos(&M, NULL)) signals(2);
     if(M.X || M.Y){
         WARNX("Mount position isn't @ zero; moving");
-        Mount.moveTo(0., 0.);
+        double zero = 0.;
+        Mount.moveTo(&zero, &zero);
         waitmoving(ncycles);
         green("Now mount @ zero\n");
     }

LibSidServo/examples/dumpmoving.c

@@ -95,10 +95,11 @@ int main(int argc, char **argv){
     signal(SIGINT, signals);  // ctrl+C - quit
     signal(SIGQUIT, signals); // ctrl+\ - quit
     signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
-    if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.)))
+    double tag = DEG2RAD(45.), zero = 0.;
+    if(MCC_E_OK != Mount.moveTo(&tag, &tag))
         ERRX("Can't move to 45, 45");
     dumpmoving(fcoords, 30., G.Ncycles);
-    Mount.moveTo(0., 0.);
+    Mount.moveTo(&zero, &zero);
     dumpmoving(fcoords, 30., G.Ncycles);
     signals(0);
     return 0;

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(MCC_E_OK != Mount.init(&Config)){
-    if(e != MCC_E_OK){
         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

LibSidServo/examples/dumpswing.c

@@ -147,23 +147,24 @@ int main(int argc, char **argv){
         tagX = 0.; tagY = DEG2RAD(G.amplitude);
     }
     double t = sl_dtime(), t0 = t;
-    double divide = 2.;
+    double divide = 2., rtagX = -tagX, rtagY = -tagY;
     for(int i = 0; i < G.Nswings; ++i){
-        Mount.moveTo(tagX, tagY);
+        Mount.moveTo(&tagX, &tagY);
         DBG("CMD: %g", sl_dtime()-t0);
         t += G.period / divide;
         divide = 1.;
         waithalf(t);
         DBG("Moved to +, t=%g", t-t0);
         DBG("CMD: %g", sl_dtime()-t0);
-        Mount.moveTo(-tagX, -tagY);
+        Mount.moveTo(&rtagX, &rtagY);
         t += G.period;
         waithalf(t);
         DBG("Moved to -, t=%g", t-t0);
         DBG("CMD: %g", sl_dtime()-t0);
     }
+    double zero = 0.;
     // be sure to move @ 0,0
-    Mount.moveTo(0., 0.);
+    Mount.moveTo(&zero, &zero);
     // wait moving ends
     pthread_join(dthr, NULL);
 #undef SCMD

LibSidServo/examples/goto.c

@@ -31,6 +31,7 @@ typedef struct{
     int help;
     int Ncycles;
     int wait;
+    int relative;
     char *coordsoutput;
     double X;
     double Y;
@@ -49,6 +50,7 @@ static sl_option_t cmdlnopts[] = {
     {"newy",        NEED_ARG,   NULL,   'Y',    arg_double, APTR(&G.Y),         "new Y coordinate"},
     {"output",      NEED_ARG,   NULL,   'o',    arg_string, APTR(&G.coordsoutput),"file to log coordinates"},
     {"wait",        NO_ARGS,    NULL,   'w',    arg_int,    APTR(&G.wait),      "wait until mowing stopped"},
+    {"relative",    NO_ARGS,    NULL,   'r',    arg_int,    APTR(&G.relative),  "relative move"},
     end_option
 };
 
@@ -100,10 +102,22 @@ int main(int _U_ argc, char _U_ **argv){
     }
     printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
     if(isnan(G.X) && isnan(G.Y)) goto out;
-    if(isnan(G.X)) G.X = RAD2DEG(M.X);
+    double *xtag = NULL, *ytag = NULL, xr, yr;
-    if(isnan(G.Y)) G.Y = RAD2DEG(M.Y);
+    if(!isnan(G.X)){
-    printf("Moving to X=%g deg, Y=%g deg\n", G.X, G.Y);
+        xr = DEG2RAD(G.X);
-    Mount.moveTo(DEG2RAD(G.X), DEG2RAD(G.Y));
+        if(G.relative) xr += M.X;
+        xtag = &xr;
+    }
+    if(!isnan(G.Y)){
+        yr = DEG2RAD(G.Y);
+        if(G.relative) yr += M.Y;
+        ytag = &yr;
+    }
+    printf("Moving to ");
+    if(xtag) printf("X=%gdeg ", G.X);
+    if(ytag) printf("Y=%gdeg", G.Y);
+    printf("\n");
+    Mount.moveTo(xtag, ytag);
     if(G.wait){
         sleep(1);
         waitmoving(G.Ncycles);

LibSidServo/libsidservo.creator.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 15.0.1, 2025-02-23T22:39:22. -->
+<!-- Written by QtCreator 15.0.1, 2025-02-24T22:23:07. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>

LibSidServo/main.c

@@ -16,6 +16,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <inttypes.h>
 #include <stdio.h>
 #include <stdlib.h>
 
@@ -30,7 +31,7 @@ conf_t Conf = {0};
  */
 static void quit(){
     DBG("Close serial devices");
-    for(int i = 0; i < 10; ++i) if(SSemergStop()) break;
+    for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break;
     closeSerial();
     DBG("Exit");
 }
@@ -73,18 +74,87 @@ static mcc_errcodes_t init(conf_t *c){
     return ret;
 }
 
+// check coordinates and speeds; return FALSE if failed
+// TODO fix to real limits!!!
+static int chkX(double X){
+    if(X > 2.*M_PI || X < -2.*M_PI) return FALSE;
+    return TRUE;
+}
+static int chkY(double Y){
+    if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE;
+    return TRUE;
+}
+static int chkXs(double s){
+    if(s < 0. || s > X_SPEED_MAX) return FALSE;
+    return TRUE;
+}
+static int chkYs(double s){
+    if(s < 0. || s > Y_SPEED_MAX) return FALSE;
+    return TRUE;
+}
+
+
 /**
  * @brief move2 - simple move to given point and stop
- * @param X - new X coordinate (radians: -pi..pi)
+ * @param X - new X coordinate (radians: -pi..pi) or NULL
- * @param Y - new Y coordinate (radians: -pi..pi)
+ * @param Y - new Y coordinate (radians: -pi..pi) or NULL
  * @return error code
  */
-static mcc_errcodes_t move2(double X, double Y){
+static mcc_errcodes_t move2(const double *X, const double *Y){
-    if(X > M_PI || X < -M_PI || Y > M_PI || Y < -M_PI){
+    if(!X && !Y) return MCC_E_BADFORMAT;
-        DBG("Wrong coords: X=%g, Y=%g", X, Y);
+    if(X){
-        return MCC_E_BADFORMAT;
+        if(!chkX(*X)) return MCC_E_BADFORMAT;
+        int64_t tag = X_RAD2MOT(*X);
+        if(!SSsetterI(CMD_MOTX, tag)) return MCC_E_FAILED;
     }
-    if(!SSXmoveto(X) || !SSYmoveto(Y)) return MCC_E_FAILED;
+    if(Y){
+        if(!chkY(*Y)) return MCC_E_BADFORMAT;
+        int64_t tag = X_RAD2MOT(*Y);
+        if(!SSsetterI(CMD_MOTY, tag)) return MCC_E_FAILED;
+    }
+    return MCC_E_OK;
+}
+
+/**
+ * @brief setspeed - set maximal speed over axis
+ * @param X (i) - max speed or NULL
+ * @param Y (i) - -//-
+ * @return errcode
+ */
+static mcc_errcodes_t setspeed(const double *X, const double *Y){
+    if(!X && !Y) return MCC_E_BADFORMAT;
+    if(X){
+        if(!chkXs(*X)) return MCC_E_BADFORMAT;
+        int32_t spd = X_RS2MOTSPD(*X);
+        if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
+    }
+    if(Y){
+        if(!chkYs(*Y)) return MCC_E_BADFORMAT;
+        int32_t spd = Y_RS2MOTSPD(*Y);
+        if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED;
+    }
+    return MCC_E_OK;
+}
+
+/**
+ * @brief move2s - move to target with given max speed
+ * @param target (i) - target or NULL
+ * @param speed (i) - speed or NULL
+ * @return
+ */
+static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){
+    if(!target && !speed) return MCC_E_BADFORMAT;
+    if(!target) return setspeed(&speed->X, &speed->Y);
+    if(!speed) return move2(&target->X, &target->Y);
+    if(!chkX(target->X) || !chkY(target->Y) || !chkXs(speed->X) || !chkYs(speed->Y))
+        return MCC_E_BADFORMAT;
+    char buf[128];
+    int32_t spd = X_RS2MOTSPD(speed->X), tag = X_RAD2MOT(target->X);
+    snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTX, tag, CMD_MOTXYS, spd);
+    if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
+    spd = Y_RS2MOTSPD(speed->Y); tag = Y_RAD2MOT(target->Y);
+    snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTY, tag, CMD_MOTXYS, spd);
+    if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
 
@@ -93,7 +163,12 @@ static mcc_errcodes_t move2(double X, double Y){
  * @return errcode
  */
 static mcc_errcodes_t emstop(){
-    if(!SSemergStop()) return MCC_E_FAILED;
+    if(!SSstop(TRUE)) return MCC_E_FAILED;
+    return MCC_E_OK;
+}
+// normal stop
+static mcc_errcodes_t stop(){
+    if(!SSstop(FALSE)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
 
@@ -159,7 +234,10 @@ mount_t Mount = {
     .quit = quit,
     .getMountData = getMD,
     .moveTo = move2,
+    .moveWspeed = move2s,
+    .setSpeed = setspeed,
     .emergStop = emstop,
+    .stop = stop,
     .shortCmd = shortcmd,
     .longCmd = longcmd,
 };

LibSidServo/serial.c

@@ -375,13 +375,14 @@ static int wr(const data_t *out, data_t *in, int needeol){
             (void) g;
         }
     }
-    if(in){
+    uint8_t buf[256];
-        in->len = 0;
+    data_t dumb = {.buf = buf, .maxlen = 256};
-        for(size_t i = 0; i < in->maxlen; ++i){
+    if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
-            int b = getmntbyte();
+    in->len = 0;
-            if(b < 0) break; // nothing to read -> go out
+    for(size_t i = 0; i < in->maxlen; ++i){
-            in->buf[in->len++] = (uint8_t) b;
+        int b = getmntbyte();
-        }
+        if(b < 0) break; // nothing to read -> go out
+        in->buf[in->len++] = (uint8_t) b;
     }
     return TRUE;
 }

LibSidServo/sidservo.h

@@ -130,7 +130,10 @@ typedef struct{
     mcc_errcodes_t  (*init)(conf_t *c); // init device
     void            (*quit)(); // deinit
     mcc_errcodes_t  (*getMountData)(mountdata_t *d); // get last data
-    mcc_errcodes_t  (*moveTo)(double X, double Y); // move to given position ans stop
+    mcc_errcodes_t  (*moveTo)(const double *X, const double *Y); // move to given position ans stop
+    mcc_errcodes_t  (*moveWspeed)(const coords_t *target, const  coords_t *speed); // move with given max speed
+    mcc_errcodes_t  (*setSpeed)(const double *X, const double *Y); // set speed
+    mcc_errcodes_t  (*stop)(); // stop
     mcc_errcodes_t  (*emergStop)(); // emergency stop
     mcc_errcodes_t  (*shortCmd)(short_command_t *cmd); // send/get short command
     mcc_errcodes_t  (*longCmd)(long_command_t *cmd); // send/get long command

LibSidServo/ssii.c

@@ -129,29 +129,25 @@ int SSgetint(const char *cmd, int64_t *ans){
     return TRUE;
 }
 
-// commands to move X and Y to given motor position in radians; @return FALSE if failed
+/**
-// BE CAREFUL: after each poweron X and Y are 0
+ * @brief SSsetterI - integer setter
-// BE CAREFUL: angle isn't checking here
+ * @param cmd - command to send
-int SSXmoveto(double pos){
+ * @param ival - value
-    char buf[64];
+ * @return false if failed
-    int64_t target = X_RAD2MOT(pos);
+ */
-    DBG("move to angle %grad = %ld", pos, target);
+int SSsetterI(const char *cmd, int32_t ival){
-    snprintf(buf, 63, "%s%" PRIi64, CMD_MOTX, target);
+    char buf[128];
-    return SStextcmd(buf, NULL);
+    snprintf(buf, 127, "%s%" PRIi64, cmd, ival);
-}
-int SSYmoveto(double pos){
-    char buf[64];
-    int64_t target = Y_RAD2MOT(pos);
-    DBG("move to angle %grad = %ld", pos, target);
-    snprintf(buf, 63, "%s%" PRIi64, CMD_MOTY, target);
     return SStextcmd(buf, NULL);
 }
 
-int SSemergStop(){
+int SSstop(int emerg){
     int i = 0;
+    const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
+    const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
     for(; i < 10; ++i){
-        if(!SStextcmd(CMD_EMSTOPX, NULL)) continue;
+        if(!SStextcmd(cmdx, NULL)) continue;
-        if(SStextcmd(CMD_EMSTOPY, NULL)) break;
+        if(SStextcmd(cmdy, NULL)) break;
     }
     if(i == 10) return FALSE;
     return TRUE;

LibSidServo/ssii.h

@@ -28,7 +28,7 @@
 #define CMD_MOTX        "X"
 #define CMD_MOTY        "Y"
 // set X/Y position with speed "sprintf(buf, "%s%d%s%d", CMD_MOTx, tagx, CMD_MOTxS, tags)
-#define CMD_MOTXS       "S"
+#define CMD_MOTXYS      "S"
 // reset current motor position to given value  (and stop, if moving)
 #define CMD_MOTXSET     "XF"
 #define CMD_MOTYSET     "YF"
@@ -173,6 +173,10 @@
 //#define Y_MOT_STEPSPERREV   (4394496.)
 #define Y_MOT_STEPSPERREV   (4394960.)
 
+// maximal speeds in rad/s: 10deg/s by X and 8deg/s by Y
+#define X_SPEED_MAX         (0.17453)
+#define Y_SPEED_MAX         (0.13963)
+
 // motor position to radians and back
 #define X_MOT2RAD(n)    (2.*M_PI * (double)n / X_MOT_STEPSPERREV)
 #define Y_MOT2RAD(n)    (2.*M_PI * (double)n / Y_MOT_STEPSPERREV)
@@ -250,7 +254,6 @@ void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *td
 int SStextcmd(const char *cmd, data_t *answer);
 int SSrawcmd(const char *cmd, data_t *answer);
 int SSgetint(const char *cmd, int64_t *ans);
-int SSXmoveto(double pos);
+int SSsetterI(const char *cmd, int32_t ival);
-int SSYmoveto(double pos);
+int SSstop(int emerg);
-int SSemergStop();
 int SSshortCmd(SSscmd *cmd);