diff --git a/LibSidServo/.qtcreator/libsidservo.creator.user b/LibSidServo/.qtcreator/libsidservo.creator.user index 230e00f..565523e 100644 --- a/LibSidServo/.qtcreator/libsidservo.creator.user +++ b/LibSidServo/.qtcreator/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/LibSidServo/TODO b/LibSidServo/TODO index c367bdc..1d2dcb7 100644 --- a/LibSidServo/TODO +++ b/LibSidServo/TODO @@ -1,2 +1,3 @@ -check angle conversions +fix encoders opening for several tries +encoderthread2() - change main cycle (remove pause, read data independently, ask for new only after timeout after last request) Read HW config even in model mode diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 230e00f..565523e 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 165a53b..a8bd16b 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -25,6 +25,7 @@ #include #include #include +#include #include "main.h" #include "movingmodel.h" @@ -225,6 +226,9 @@ static mcc_errcodes_t init(conf_t *c){ // read HW config to update constants hardware_configuration_t HW; if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED; + // make a pause for actual encoder's values + double t0 = timefromstart(); + while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000); return updateMotorPos(); } @@ -489,15 +493,19 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){ Yticks = ((double) i64) / 4.; X_ENC_ZERO = Conf.XEncZero; Y_ENC_ZERO = Conf.YEncZero; - X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = (config.xbits.motrev) ? -Xticks : Xticks; - Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = (config.ybits.motrev) ? -Yticks : Yticks; + DBG("xyrev: %d/%d", config.xbits.motrev, config.ybits.motrev); + X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = Xticks; // (config.xbits.motrev) ? -Xticks : Xticks; + Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = Yticks; //(config.ybits.motrev) ? -Yticks : Yticks; + DBG("zero: %d/%d; motsteps: %.10g/%.10g", X_ENC_ZERO, Y_ENC_ZERO, X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV); // axis encoder ticks per rev if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED; Xticks = (double) i64; if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED; Yticks = (double) i64; + DBG("xyencrev: %d/%d", config.xbits.encrev, config.ybits.encrev); X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks; Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks; + DBG("encsteps: %.10g/%.10g", X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV); return MCC_E_OK; } diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index ad4997e..987e2da 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -48,7 +48,7 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER, // encoders thread and mount thread static pthread_t encthread, mntthread; // max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select` -static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 5000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; +static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; // encoders raw data typedef struct __attribute__((packed)){ uint8_t magick; @@ -151,32 +151,52 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){ * @return amount of data read or 0 if problem */ static int getencval(int fd, double *val, struct timespec *t){ - if(fd < 0) return FALSE; + if(fd < 0){ + DBG("Encoder fd < 0!"); + return FALSE; + } char buf[128]; int got = 0, Lmax = 127; double t0 = timefromstart(); + //DBG("start: %.6g", t0); 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){ + //DBG("select()==0 - timeout, %.6g", timefromstart()); + break; + } if(retval < 0){ - if(errno == EINTR) continue; + if(errno == EINTR){ + DBG("EINTR"); + continue; + } + DBG("select() < 0"); return 0; } if(FD_ISSET(fd, &rfds)){ ssize_t l = read(fd, &buf[got], Lmax); - if(l < 1) return 0; // disconnected ?? + if(l < 1){ + DBG("read() < 0"); + return 0; // disconnected ?? + } got += l; Lmax -= l; buf[got] = 0; } else continue; - if(strchr(buf, '\n')) break; - }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval); - if(got == 0) return 0; // WTF? + if(buf[got-1] == '\n') break; // got EOL as last symbol + }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.); + if(got == 0){ + //DBG("No data from encoder, tfs=%.6g", timefromstart()); + return 0; + } char *estr = strrchr(buf, '\n'); - if(!estr) return 0; + if(!estr){ + DBG("No EOL"); + return 0; + } *estr = 0; char *bgn = strrchr(buf, '\n'); if(bgn) ++bgn; @@ -188,7 +208,7 @@ static int getencval(int fd, double *val, struct timespec *t){ return 0; // wrong number } if(val) *val = (double) data; - if(t){ if(!curtime(t)) return 0; } + if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }} return got; } // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected @@ -312,10 +332,17 @@ static void *encoderthread2(void _U_ *u){ int need2ask = 0; // need or not to ask encoder for new data while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){ struct timespec t; - if(!curtime(&t)) continue; if(need2ask){ + //DBG("ASK for new data, tfs=%.6g", timefromstart()); if(1 != write(encfd[0], req, 1)) { ++errctr; continue; } else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; } + //DBG("OK"); + need2ask = 0; + usleep(100); + } + if(!curtime(&t)){ + DBG("Where is time?"); + continue; } double v; if(getencval(encfd[0], &v, &t)){ @@ -323,29 +350,38 @@ static void *encoderthread2(void _U_ *u){ mountdata.encXposition.val = Xenc2rad(v); mountdata.encXposition.t = t; pthread_mutex_unlock(&datamutex); + //DBG("MDXpos: %.10g/%g (xez=%d, xesr=%.10g), tfs=%.6g", v, mountdata.encXposition.val, X_ENC_ZERO, X_ENC_STEPSPERREV, timefromstart()); getXspeed(); if(getencval(encfd[1], &v, &t)){ pthread_mutex_lock(&datamutex); mountdata.encYposition.val = Yenc2rad(v); mountdata.encYposition.t = t; pthread_mutex_unlock(&datamutex); + //DBG("MDYpos: %.10g/%g (yez=%d, yesr=%.10g)", v, mountdata.encYposition.val, Y_ENC_ZERO, Y_ENC_STEPSPERREV); getYspeed(); errctr = 0; need2ask = 0; - } else { - if(need2ask) ++errctr; + //DBG("tgot=%.6g", timefromstart()); + while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); } + t0 = timefromstart(); + }else{ + DBG("NO Y DATA!!!"); + /*if(need2ask) ++errctr; else need2ask = 1; - continue; + continue;*/ + ++errctr; + need2ask = 1; } - } else { - if(need2ask) ++errctr; + }else{ // no data - ask for new + //DBG("Need new, tfs=%.6g", timefromstart()); + /*if(need2ask) ++errctr; else need2ask = 1; - continue; + continue;*/ + ++errctr; + need2ask = 1; } - while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); } - t0 = timefromstart(); } - DBG("ERRCTR=%d", errctr); + DBG("\n\nEXIT: ERRCTR=%d", errctr); for(int i = 0; i < 2; ++i){ if(encfd[i] > -1){ close(encfd[i]); @@ -480,8 +516,15 @@ static int ttyopen(const char *path, speed_t speed){ int fd = -1; struct termios2 tty; DBG("Try to open %s @ %d", path, speed); - if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1; - if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; } + if((fd = open(path, O_RDWR|O_NOCTTY)) < 0){ + DBG("Can't open device %s: %s", path, strerror(errno)); + return -1; + } + if(ioctl(fd, TCGETS2, &tty)){ + DBG("Can't read TTY settings"); + close(fd); + return -1; + } tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG) tty.c_iflag = 0; // don't do any changes in input stream tty.c_oflag = 0; // don't do any changes in output stream @@ -490,7 +533,11 @@ static int ttyopen(const char *path, speed_t speed){ tty.c_ospeed = speed; //tty.c_cc[VMIN] = 0; // non-canonical mode //tty.c_cc[VTIME] = 5; - if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; } + if(ioctl(fd, TCSETS2, &tty)){ + DBG("Can't set TTY settings"); + close(fd); + return -1; + } 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 @@ -523,7 +570,7 @@ int openEncoder(){ if(encfd[i] < 0) return FALSE; } encRtmout.tv_sec = 0; - encRtmout.tv_usec = 1000; // 1ms + encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed; if(pthread_create(&encthread, NULL, encoderthread2, NULL)){ for(int i = 0; i < 2; ++i){ close(encfd[i]); @@ -602,6 +649,8 @@ mcc_errcodes_t getMD(mountdata_t *d){ pthread_mutex_lock(&datamutex); *d = mountdata; pthread_mutex_unlock(&datamutex); + //DBG("ENCpos: %.10g/%.10g", d->encXposition.val, d->encYposition.val); + //DBG("millis: %u, encxt: %zd (time: %zd)", d->millis, d->encXposition.t.tv_sec, time(NULL)); return MCC_E_OK; } diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index 23a6fb5..2c88e52 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -79,6 +79,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){ // fill encoder data from here, as there's no separate enc thread if(!Conf.SepEncoder){ m->encXposition.val = Xenc2rad(s->Xenc); + DBG("encx: %g", m->encXposition.val); m->encYposition.val = Yenc2rad(s->Yenc); m->encXposition.t = m->encYposition.t = *t; getXspeed(); getYspeed(); @@ -188,9 +189,10 @@ mcc_errcodes_t updateMotorPos(){ usleep(10000); continue; } + //DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV); if(MCC_E_OK == getMD(&md)){ if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){ - DBG("Just started, t-t0 = %g!", t - t0); + DBG("Just started? t-t0 = %g!", t - t0); usleep(10000); continue; } @@ -198,14 +200,15 @@ mcc_errcodes_t updateMotorPos(){ DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec); mcc_errcodes_t OK = MCC_E_OK; if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){ - DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val); + DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val); + DBG("new motsteps: %d", X_RAD2MOT(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", t - t0); } if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){ - DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val); + DBG("NEED to sync Y: motors=%g, axis=%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 1ba3441..39d65a0 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -199,44 +199,46 @@ extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STE // encoder reversed (no: +1) -> sign of ...stepsperrev //#define X_ENC_SIGN (-1.) //#define Y_ENC_SIGN (-1.) + + // encoder position to radians and back -#define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV) -#define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV) -#define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV)) -#define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV)) +#define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV)) +#define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV)) +#define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV))) +#define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV))) // convert angle in radians to +-pi -static inline double ang2half(double ang){ +static inline __attribute__((always_inline)) double ang2half(double ang){ if(ang < -M_PI) ang += 2.*M_PI; else if(ang > M_PI) ang -= 2.*M_PI; return ang; } // convert to only positive: 0..2pi -static inline double ang2full(double ang){ +static inline __attribute__((always_inline)) double ang2full(double ang){ if(ang < 0.) ang += 2.*M_PI; else if(ang > 2.*M_PI) ang -= 2.*M_PI; return ang; } // motor position to radians and back -#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV) -#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV) -#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV)) -#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV)) +#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (X_MOT_STEPSPERREV)) +#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (Y_MOT_STEPSPERREV)) +#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (X_MOT_STEPSPERREV))) +#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (Y_MOT_STEPSPERREV))) // motor speed in rad/s and back -#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY) -#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY) -#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY)) -#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY)) +#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY)) +#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY)) +#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY))) +#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY))) // motor acceleration -//- -#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY) -#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY) -#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY)) -#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY)) +#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY)) +#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY)) +#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY))) +#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY))) // adder time to seconds vice versa -#define ADDER2S(a) ((a) / SITECH_LOOP_FREQUENCY) -#define S2ADDER(s) ((s) * SITECH_LOOP_FREQUENCY) +#define ADDER2S(a) ((a) / (SITECH_LOOP_FREQUENCY)) +#define S2ADDER(s) ((s) * (SITECH_LOOP_FREQUENCY)) // encoder's tolerance (ticks) #define YencTOL (25.)