add new encoders' controller

LibSidServo/examples/conf.c

@@ -24,10 +24,12 @@
 static conf_t Config = {
     .MountDevPath = "/dev/ttyUSB0",
     .MountDevSpeed = 19200,
-    .EncoderDevPath = "/dev/ttyUSB1",
+    .EncoderXDevPath = "/dev/encoderX0",
+    .EncoderYDevPath = "/dev/encoderY0",
     .EncoderDevSpeed = 153000,
     .MountReqInterval = 0.1,
-    .SepEncoder = 1
+    .EncoderReqInterval = 0.05,
+    .SepEncoder = 2
 };
 
 static sl_option_t opts[] = {
@@ -36,7 +38,10 @@ static sl_option_t opts[] = {
     {"EncoderDevPath",  NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderDevPath),   "path to encoder device"},
     {"EncoderDevSpeed", NEED_ARG,   NULL,   0,  arg_int,    APTR(&Config.EncoderDevSpeed),  "serial speed of encoder device"},
     {"MountReqInterval",NEED_ARG,   NULL,   0,  arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
-    {"SepEncoder",      NO_ARGS,    NULL,   0,  arg_int,    APTR(&Config.SepEncoder),       "encoder is separate device"},
+    {"EncoderReqInterval",NEED_ARG, NULL,   0,  arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
+    {"SepEncoder",      NO_ARGS,    NULL,   0,  arg_int,    APTR(&Config.SepEncoder),       "encoder is separate device (1 - one device, 2 - two devices)"},
+    {"EncoderXDevPath", NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderXDevPath),  "path to X encoder (/dev/encoderX0)"},
+    {"EncoderYDevPath", NEED_ARG,   NULL,   0,  arg_string, APTR(&Config.EncoderYDevPath),  "path to Y encoder (/dev/encoderY0)"},
     end_option
 };
 

LibSidServo/examples/dumpmoving_scmd.c

@@ -77,8 +77,8 @@ static void *dumping(void _U_ *u){
     return NULL;
 }
 
-// return TRUE if motor position is reached +- 0.1 degrees
+// return TRUE if motor position is reached +- 0.01 degrees
-#define XYcount (DEG2RAD(0.1))
+#define XYcount (DEG2RAD(0.01))
 static int Wait(double tag){
     mountdata_t mdata;
     red("Wait for %g degrees\n", RAD2DEG(tag));
@@ -104,6 +104,7 @@ static int Wait(double tag){
         return FALSE;
     }
     green("%s reached position %g degrees\n", G.axis, RAD2DEG(tag));
+    fflush(stdout);
     return TRUE;
 }
 
@@ -120,7 +121,7 @@ static void move(double target, double limit, double speed){
     if(*G.axis == 'Y' || *G.axis == 'B'){
         cmd.Ymot = DEG2RAD(target) + M.Y;
         cmd.Yspeed = DEG2RAD(speed);
-        limit = DEG2RAD(limit) + M.Y;
+        if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y;
     }
     SCMD();
     if(!Wait(limit)) signals(9);
@@ -160,16 +161,14 @@ int main(int argc, char **argv){
     pthread_t dthr;
     logmnt(fcoords, NULL);
     if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
-    // goto 1 degr with 1'/s
+    // goto 30' with 5'/s
-    move(10., 1., 1./60.);
+    move(10., 30./60., 5./60.);
-    // goto 2 degr with 2'/s
+    // goto 1' with 10'/s
-    move(10., 2., 2./60.);
+    move(10., 1., 10./60.);
-    // goto 3 degr with 5'/s
+    // goto 3degr with 15'/s
-    move(10., 3., 5./60.);
+    move(10., 3., 15./60.);
-    // goto 4 degr with 10'/s
+    // and go back with 7deg/s
-    move(10., 4., 10./60.);
+    move(0., 0., 7.);
-    // and go back with 5deg/s
-    move(0., 0., 5.);
     // be sure to move @ 0,0
     Mount.moveTo(&M.X, &M.Y);
     // wait moving ends

LibSidServo/examples/goto.c

@@ -103,15 +103,19 @@ 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;
     double *xtag = NULL, *ytag = NULL, xr, yr;
+    double _7deg = RAD2DEG(7.);
     if(!isnan(G.X)){
         xr = DEG2RAD(G.X);
         if(G.relative) xr += M.X;
         xtag = &xr;
+        // set max speed
+        Mount.setSpeed(&_7deg, NULL);
     }
     if(!isnan(G.Y)){
         yr = DEG2RAD(G.Y);
         if(G.relative) yr += M.Y;
         ytag = &yr;
+        Mount.setSpeed(NULL, &_7deg);
     }
     printf("Moving to ");
     if(xtag) printf("X=%gdeg ", G.X);

LibSidServo/examples/servo.conf

@@ -0,0 +1,8 @@
+MountDevPath=/dev/ttyUSB0
+MountDevSpeed=19200
+EncoderXDevPath=/dev/encoder_X0
+EncoderYDevPath=/dev/encoder_Y0
+MountReqInterval=0.05
+SepEncoder=2
+EncoderReqInterval=0.01
+EncoderDevSpeed=1000000

LibSidServo/main.c

@@ -49,16 +49,16 @@ static mcc_errcodes_t init(conf_t *c){
     if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
         DBG("Define mount device path and speed");
         ret = MCC_E_BADFORMAT;
-    }else if(!openMount(Conf.MountDevPath, Conf.MountDevSpeed)){
+    }else if(!openMount()){
         DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed);
         ret = MCC_E_MOUNTDEV;
     }
     if(Conf.SepEncoder){
-        if(!Conf.EncoderDevPath || Conf.EncoderDevSpeed < 1200){
+        if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){
-            DBG("Define encoder device path and speed");
+            DBG("Define encoder device path");
             ret = MCC_E_BADFORMAT;
-        }else if(!openEncoder(Conf.EncoderDevPath, Conf.EncoderDevSpeed)){
+        }else if(!openEncoder()){
-            DBG("Can't open %s with speed %d", Conf.EncoderDevPath, Conf.EncoderDevSpeed);
+            DBG("Can't open encoder device");
             ret = MCC_E_ENCODERDEV;
         }
     }
@@ -152,10 +152,10 @@ static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){
         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);
+    snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, 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);
+    snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, CMD_MOTY, tag, CMD_MOTXYS, spd);
     if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
     return MCC_E_OK;
 }
@@ -342,6 +342,7 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
     // Convert backlash speed (rad/s to ticks per loop)
     config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
     // TODO - next
+    (void) config;
     return MCC_E_OK;
 }
 

LibSidServo/serial.c

@@ -34,7 +34,7 @@
 #include "serial.h"
 
 // serial devices FD
-static int encfd = -1, mntfd = -1;
+static int encfd[2] = {-1, -1}, mntfd = -1;
 // main mount data
 static mountdata_t mountdata = {0};
 
@@ -122,24 +122,72 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
     //DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
 }
 
+/**
+ * @brief getencval - get uint64_t data from encoder
+ * @param fd - encoder fd
+ * @param val - value read
+ * @param tv - measurement time
+ * @return amount of data read or 0 if problem
+ */
+static int getencval(int fd, double *val, struct timeval *tv){
+    if(fd < 0) return FALSE;
+    char buf[128];
+    int got = 0, Lmax = 127;
+    double t0 = dtime();
+    do{
+        fd_set rfds;
+        FD_ZERO(&rfds);
+        FD_SET(fd, &rfds);
+        struct timeval tv = encRtmout;
+        int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
+        if(!retval) continue;
+        if(retval < 0){
+            if(errno == EINTR) continue;
+            return 0;
+        }
+        if(FD_ISSET(fd, &rfds)){
+            ssize_t l = read(fd, &buf[got], Lmax);
+            if(l < 1) return 0; // disconnected ??
+            got += l; Lmax -= l;
+            buf[got] = 0;
+        } else continue;
+        if(strchr(buf, '\n')) break;
+    }while(Lmax && dtime() - t0 > Conf.EncoderReqInterval);
+    if(got == 0) return 0; // WTF?
+    char *estr = strrchr(buf, '\n');
+    if(!estr) return 0;
+    *estr = 0;
+    char *bgn = strrchr(buf, '\n');
+    if(bgn) ++bgn;
+    else bgn = buf;
+    char *eptr;
+    long data = strtol(bgn, &eptr, 10);
+    if(eptr != estr){
+        DBG("NAN");
+        return 0; // wrong number
+    }
+    if(val) *val = (double) data;
+    if(tv) gettimeofday(tv, NULL);
+    return got;
+}
 // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
 static int getencbyte(){
-    if(encfd < 0) return -1;
+    if(encfd[0] < 0) return -1;
     uint8_t byte = 0;
     fd_set rfds;
     struct timeval tv;
     do{
         FD_ZERO(&rfds);
-        FD_SET(encfd, &rfds);
+        FD_SET(encfd[0], &rfds);
         tv = encRtmout;
-        int retval = select(encfd + 1, &rfds, NULL, NULL, &tv);
+        int retval = select(encfd[0] + 1, &rfds, NULL, NULL, &tv);
         if(!retval) break;
         if(retval < 0){
             if(errno == EINTR) continue;
             return -1;
         }
-        if(FD_ISSET(encfd, &rfds)){
+        if(FD_ISSET(encfd[0], &rfds)){
-            ssize_t l = read(encfd, &byte, 1);
+            ssize_t l = read(encfd[0], &byte, 1);
             if(l != 1) return -2; // disconnected ??
             break;
         } else return -1;
@@ -179,11 +227,12 @@ static int getmntbyte(){
 }
 
 // main encoder thread (for separate encoder): read next data and make parsing
-static void *encoderthread(void _U_ *u){
+static void *encoderthread1(void _U_ *u){
+    if(Conf.SepEncoder != 1) return NULL;
     uint8_t databuf[ENC_DATALEN];
     int wridx = 0, errctr = 0;
     struct timeval tv;
-    while(encfd > -1 && errctr < MAX_ERR_CTR){
+    while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
         int b = getencbyte();
         if(b == -2) ++errctr;
         if(b < 0) continue;
@@ -202,9 +251,42 @@ static void *encoderthread(void _U_ *u){
             wridx = 0;
         }
     }
-    if(encfd > -1){
+    if(encfd[0] > -1){
-        close(encfd);
+        close(encfd[0]);
-        encfd = -1;
+        encfd[0] = -1;
+    }
+    return NULL;
+}
+
+static void *encoderthread2(void _U_ *u){
+    if(Conf.SepEncoder != 2) return NULL;
+    DBG("Thread started");
+    int errctr = 0;
+    double t0 = dtime();
+    const char *req = "next\n";
+    while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
+        if(5 != write(encfd[0], req, 5)) ++errctr;
+        if(5 != write(encfd[1], req, 5)) ++errctr;
+        double v;
+        struct timeval tv;
+        if(getencval(encfd[0], &v, &tv)){
+            mountdata.encposition.X = X_ENC2RAD(v);
+            mountdata.encposition.msrtime = tv;
+            if(getencval(encfd[1], &v, &tv)){
+                mountdata.encposition.Y = Y_ENC2RAD(v);
+                mountdata.encposition.msrtime = tv;
+                errctr = 0;
+            } else ++errctr;
+        } else ++errctr;
+        while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); }
+        //DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval);
+        t0 = dtime();
+    }
+    for(int i = 0; i < 2; ++i){
+        if(encfd[i] > -1){
+            close(encfd[i]);
+            encfd[i] = -1;
+        }
     }
     return NULL;
 }
@@ -293,7 +375,7 @@ static int ttyopen(const char *path, speed_t speed){
     //tty.c_cc[VMIN]  = 0;  // non-canonical mode
     //tty.c_cc[VTIME] = 5;
     if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; }
-    DBG("Check speed");
+    DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed);
     if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
     // try to set exclusive
     if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
@@ -301,27 +383,47 @@ static int ttyopen(const char *path, speed_t speed){
 }
 
 // return FALSE if failed
-int openEncoder(const char *path, int speed){
+int openEncoder(){
     if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
-    if(encfd > -1) close(encfd);
+    if(Conf.SepEncoder == 1){ // only one device
-    encfd = ttyopen(path, (speed_t) speed);
+        DBG("One device");
-    if(encfd < 0) return FALSE;
+        if(encfd[0] > -1) close(encfd[0]);
-    encRtmout.tv_sec = 0;
+        encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
-    encRtmout.tv_usec = 200000000 / speed; // 20 bytes
+        if(encfd[0] < 0) return FALSE;
-    if(pthread_create(&encthread, NULL, encoderthread, NULL)){
+        encRtmout.tv_sec = 0;
-        close(encfd);
+        encRtmout.tv_usec = 200000000 / Conf.EncoderDevSpeed; // 20 bytes
-        encfd = -1;
+        if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
-        return FALSE;
+            close(encfd[0]);
-    }
+            encfd[0] = -1;
+            return FALSE;
+        }
+    }else if(Conf.SepEncoder == 2){
+        DBG("Two devices!");
+        const char* paths[2] = {Conf.EncoderXDevPath, Conf.EncoderYDevPath};
+        for(int i = 0; i < 2; ++i){
+            if(encfd[i] > -1) close(encfd[i]);
+            encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed);
+            if(encfd[i] < 0) return FALSE;
+        }
+        encRtmout.tv_sec = 0;
+        encRtmout.tv_usec = 1000; // 1ms
+        if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
+            for(int i = 0; i < 2; ++i){
+                close(encfd[i]);
+                encfd[i] = -1;
+            }
+            return FALSE;
+        }
+    }else return FALSE;
     DBG("Encoder opened, thread started");
     return TRUE;
 }
 
 // return FALSE if failed
-int openMount(const char *path, int speed){
+int openMount(){
     if(mntfd > -1) close(mntfd);
-    DBG("Open mount %s @ %d", path, speed);
+    DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
-    mntfd = ttyopen(path, (speed_t) speed);
+    mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
     if(mntfd < 0) return FALSE;
     DBG("mntfd=%d", mntfd);
     // clear buffer
@@ -334,7 +436,7 @@ int openMount(const char *path, int speed){
         DBG("got %zd", l);
     }while(1);*/
     mntRtmout.tv_sec = 0;
-    mntRtmout.tv_usec = 500000000 / speed; // 50 bytes
+    mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes
     if(pthread_create(&mntthread, NULL, mountthread, NULL)){
         DBG("Can't create thread");
         close(mntfd);
@@ -354,12 +456,16 @@ void closeSerial(){
         close(mntfd);
         mntfd = -1;
     }
-    if(encfd > -1){
+    if(encfd[0] > -1){
         DBG("Kill encoder thread");
         pthread_cancel(encthread);
         DBG("close fd");
-        close(encfd);
+        close(encfd[0]);
-        encfd = -1;
+        encfd[0] = -1;
+        if(Conf.SepEncoder == 2){
+            close(encfd[1]);
+            encfd[1] = -1;
+        }
     }
 }
 

LibSidServo/serial.h

@@ -31,8 +31,8 @@
 double dtime();
 data_t *cmd2dat(const char *cmd);
 void data_free(data_t **x);
-int openEncoder(const char *path, int speed);
+int openEncoder();
-int openMount(const char *path, int speed);
+int openMount();
 void closeSerial();
 mcc_errcodes_t getMD(mountdata_t  *d);
 int MountWriteRead(const data_t *out, data_t *in);

LibSidServo/sidservo.h

@@ -42,8 +42,11 @@ typedef struct{
     int     MountDevSpeed;      // serial speed
     char*   EncoderDevPath;     // path to encoder device
     int     EncoderDevSpeed;    // serial speed
-    int     SepEncoder;         // ==1 if encoder works as separate serial device
+    int     SepEncoder;         // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
-    double  MountReqInterval;   // maximal interval between subsequent mount requests (seconds)
+    char*   EncoderXDevPath;    // paths to new controller devices
+    char*   EncoderYDevPath;
+    double  MountReqInterval;   // interval between subsequent mount requests (seconds)
+    double  EncoderReqInterval; // interval between subsequent encoder requests (seconds)
     ;
 } conf_t;