From 8ca8183cb895037ec50d163bdc6250302517de65 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Tue, 8 Apr 2025 22:47:46 +0300 Subject: [PATCH] add new encoders' controller --- LibSidServo/examples/conf.c | 11 +- LibSidServo/examples/dumpmoving_scmd.c | 25 ++-- LibSidServo/examples/goto.c | 4 + LibSidServo/examples/servo.conf | 8 ++ LibSidServo/main.c | 15 +-- LibSidServo/serial.c | 166 ++++++++++++++++++++----- LibSidServo/serial.h | 4 +- LibSidServo/sidservo.h | 7 +- 8 files changed, 183 insertions(+), 57 deletions(-) create mode 100644 LibSidServo/examples/servo.conf diff --git a/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c index 428bec4..db731ea 100644 --- a/LibSidServo/examples/conf.c +++ b/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 }; diff --git a/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c index ec9a029..6081f68 100644 --- a/LibSidServo/examples/dumpmoving_scmd.c +++ b/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 -#define XYcount (DEG2RAD(0.1)) +// return TRUE if motor position is reached +- 0.01 degrees +#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 - move(10., 1., 1./60.); - // goto 2 degr with 2'/s - move(10., 2., 2./60.); - // goto 3 degr with 5'/s - move(10., 3., 5./60.); - // goto 4 degr with 10'/s - move(10., 4., 10./60.); - // and go back with 5deg/s - move(0., 0., 5.); + // goto 30' with 5'/s + move(10., 30./60., 5./60.); + // goto 1' with 10'/s + move(10., 1., 10./60.); + // goto 3degr with 15'/s + move(10., 3., 15./60.); + // and go back with 7deg/s + move(0., 0., 7.); // be sure to move @ 0,0 Mount.moveTo(&M.X, &M.Y); // wait moving ends diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c index 3da2b9a..7c4f843 100644 --- a/LibSidServo/examples/goto.c +++ b/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); diff --git a/LibSidServo/examples/servo.conf b/LibSidServo/examples/servo.conf new file mode 100644 index 0000000..d37e959 --- /dev/null +++ b/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 diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 3e8076c..2d3a7aa 100644 --- a/LibSidServo/main.c +++ b/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){ - DBG("Define encoder device path and speed"); + if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){ + DBG("Define encoder device path"); ret = MCC_E_BADFORMAT; - }else if(!openEncoder(Conf.EncoderDevPath, Conf.EncoderDevSpeed)){ - DBG("Can't open %s with speed %d", Conf.EncoderDevPath, Conf.EncoderDevSpeed); + }else if(!openEncoder()){ + 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; } diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 3f4b823..ab6b8cf 100644 --- a/LibSidServo/serial.c +++ b/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)){ - ssize_t l = read(encfd, &byte, 1); + if(FD_ISSET(encfd[0], &rfds)){ + 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){ - close(encfd); - encfd = -1; + if(encfd[0] > -1){ + close(encfd[0]); + 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); - encfd = ttyopen(path, (speed_t) speed); - if(encfd < 0) return FALSE; - encRtmout.tv_sec = 0; - encRtmout.tv_usec = 200000000 / speed; // 20 bytes - if(pthread_create(&encthread, NULL, encoderthread, NULL)){ - close(encfd); - encfd = -1; - return FALSE; - } + if(Conf.SepEncoder == 1){ // only one device + DBG("One device"); + if(encfd[0] > -1) close(encfd[0]); + encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed); + if(encfd[0] < 0) return FALSE; + encRtmout.tv_sec = 0; + encRtmout.tv_usec = 200000000 / Conf.EncoderDevSpeed; // 20 bytes + if(pthread_create(&encthread, NULL, encoderthread1, NULL)){ + 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); - mntfd = ttyopen(path, (speed_t) speed); + DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed); + 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); - encfd = -1; + close(encfd[0]); + encfd[0] = -1; + if(Conf.SepEncoder == 2){ + close(encfd[1]); + encfd[1] = -1; + } } } diff --git a/LibSidServo/serial.h b/LibSidServo/serial.h index aa2604c..95b5e8d 100644 --- a/LibSidServo/serial.h +++ b/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 openMount(const char *path, int speed); +int openEncoder(); +int openMount(); void closeSerial(); mcc_errcodes_t getMD(mountdata_t *d); int MountWriteRead(const data_t *out, data_t *in); diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 53bac57..8e01bfb 100644 --- a/LibSidServo/sidservo.h +++ b/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 - double MountReqInterval; // maximal interval between subsequent mount requests (seconds) + int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices + 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;