diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user index e6b41cb..58c6b84 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/Auxiliary_utils/LibSidServo/libsidservo.files b/Auxiliary_utils/LibSidServo/libsidservo.files index 7777ac7..aa9288a 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.files +++ b/Auxiliary_utils/LibSidServo/libsidservo.files @@ -22,6 +22,7 @@ examples/traectories.h main.h movingmodel.c movingmodel.h +polltest/main.c ramp.c ramp.h serial.h diff --git a/Auxiliary_utils/LibSidServo/polltest/main.c b/Auxiliary_utils/LibSidServo/polltest/main.c new file mode 100644 index 0000000..b67ad0e --- /dev/null +++ b/Auxiliary_utils/LibSidServo/polltest/main.c @@ -0,0 +1,237 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2026 Edward V. Emelianov . + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include +#include +#include + +#define XYBUFSZ (2048) + +struct{ + int help; + char *Xpath; + char *Ypath; + double dt; +} G = { + .Xpath = "/dev/encoder_X0", + .Ypath = "/dev/encoder_Y0", + .dt = 0.001, +}; + +sl_option_t options[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"Xpath", NEED_ARG, NULL, 'X', arg_string, APTR(&G.Xpath), "path to X encoder"}, + {"Ypath", NEED_ARG, NULL, 'Y', arg_string, APTR(&G.Ypath), "path to Y encoder"}, + {"dt", NEED_ARG, NULL, 'd', arg_double, APTR(&G.dt), "request interval (1e-4..10s)"}, +}; + +typedef struct{ + char buf[XYBUFSZ+1]; + int len; +} buf_t; + +static int Xfd = -1, Yfd = -1; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + DBG("close"); + if(Xfd > 0){ close(Xfd); Xfd = -1; } + if(Yfd > 0){ close(Yfd); Yfd = -1; } + exit(sig); +} + +static int op(const char *nm){ + int fd = open(nm, O_RDWR|O_NOCTTY|O_NONBLOCK); + if(fd < 0) ERR("Can't open %s", nm); + struct termios2 tty; + if(ioctl(fd, TCGETS2, &tty)) ERR("Can't read TTY settings"); + 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 + tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl + tty.c_ispeed = 1000000; + tty.c_ospeed = 1000000; + if(ioctl(fd, TCSETS2, &tty)) ERR("Can't set TTY settings"); + // try to set exclusive + if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");} + return fd; +} + +static int eolcnt(buf_t *buf){ + if(!buf) return -1; + int cnt = 0; + for(int i = 0; i < buf->len; ++i) + if(buf->buf[i] == '\n') ++cnt; + return cnt; +} +// move last record (if any) into head of buffer +static void movelast(buf_t *buf){ + FNAME(); + if(!buf) return; + DBG("buf was: %s", buf->buf); + int cnt = eolcnt(buf); + char *E = strrchr(buf->buf, '\n'); + int idx = -1; + if(E){ + idx = ++E - buf->buf; // position of symbol after '\n' + }else{ + buf->len = strlen(buf->buf); + DBG("leave as is (%s)", buf->buf); + return; + } + DBG("cnt=%d, idx=%d", cnt, idx); + switch(cnt){ + case 0: // EOL not found - clear buf + buf->len = 0; + break; + case 1: // only one record - move all after '\n' + if(idx > 0 && idx < XYBUFSZ){ + buf->len = XYBUFSZ - idx; + memmove(buf->buf, E, buf->len); + }else buf->len = 0; + break; + default: // more than one record - move + { + int i = idx - 2; + for(; i > -1; --i) + if(buf->buf[i] == '\n') break; + ++i; + buf->len = XYBUFSZ - i; + memmove(buf->buf, &buf->buf[i], buf->len); + } + } + buf->buf[buf->len] = 0; + DBG("MOVED; now buf[%d]=%s", buf->len, buf->buf); +} +// write to buffer next data portion; return FALSE in case of error +static int readstrings(buf_t *buf, int fd){ + if(!buf){WARNX("Empty buffer"); return FALSE;} + int L = XYBUFSZ - buf->len; + if(L == 0){ + DBG("len: %d", buf->len); + movelast(buf); + L = XYBUFSZ - buf->len; + } + int got = read(fd, &buf->buf[buf->len], L); + if(got < 0){ + WARN("read()"); + return FALSE; + }else if(got == 0) return TRUE; + buf->len += got; + buf->buf[buf->len] = 0; + DBG("buf[%d]: %s", buf->len, buf->buf); + return TRUE; +} +// return TRUE if got, FALSE if no data found +static int getdata(buf_t *buf, long *out){ + if(!buf) return -1; + // read record between last '\n' and previous (or start of string) + int cnt = eolcnt(buf); + if(cnt < 1) return FALSE; + char *last = strrchr(buf->buf, '\n'); + if(!last) return FALSE; // WTF? + *last = 0; + char *prev = buf->buf; + if(cnt > 1) prev = strrchr(buf->buf, '\n') + 1; + if(!prev) prev = buf->buf; // ?? + if(out) *out = atol(prev); + int l = strlen(++last); + if(l < XYBUFSZ){ + buf->len = l; + if(l){ + memmove(buf->buf, last, l); + DBG("moved: %s", buf->buf); + }else DBG("empty line"); + }else{ + buf->len = 0; + DBG("buffer clear"); + } + return TRUE; +} +// try to write '\n' asking new data portion; return FALSE if failed +static int asknext(int fd){ + FNAME(); + if(fd < 0) return FALSE; + int i = 0; + for(; i < 5; ++i){ + int l = write(fd, "\n", 1); + DBG("l=%d", l); + if(1 == l) return TRUE; + usleep(100); + } + DBG("5 tries... failed!"); + return FALSE; +} + +int main(int argc, char **argv){ + buf_t xbuf, ybuf; + long xlast, ylast; + double xtlast, ytlast; + sl_init(); + sl_parseargs(&argc, &argv, options); + if(G.help) sl_showhelp(-1, options); + if(G.dt < 1e-4) ERRX("dx too small"); + if(G.dt > 10.) ERRX("dx too big"); + Xfd = op(G.Xpath); + Yfd = op(G.Ypath); + struct pollfd pfds[2]; + pfds[0].fd = Xfd; pfds[0].events = POLLIN; + pfds[1].fd = Yfd; pfds[1].events = POLLIN; + double t0x, t0y, tstart; + asknext(Xfd); asknext(Yfd); + t0x = t0y = tstart = sl_dtime(); + DBG("Start"); + do{ // main cycle + if(poll(pfds, 2, 1) < 0){ + WARN("poll()"); + break; + } + if(pfds[0].revents && POLLIN){ + if(!readstrings(&xbuf, Xfd)) break; + } + if(pfds[1].revents && POLLIN){ + if(!readstrings(&ybuf, Yfd)) break; + } + double curt = sl_dtime(); + if(getdata(&xbuf, &xlast)) xtlast = curt; + if(curt - t0x >= G.dt){ // get last records + if(curt - xtlast < 1.5*G.dt) + printf("%-14.4fX=%ld\n", xtlast-tstart, xlast); + if(!asknext(Xfd)) break; + t0x = (curt - t0x < 2.*G.dt) ? t0x + G.dt : curt; + } + curt = sl_dtime(); + if(getdata(&ybuf, &ylast)) ytlast = curt; + if(curt - t0y >= G.dt){ // get last records + if(curt - ytlast < 1.5*G.dt) + printf("%-14.4fY=%ld\n", ytlast-tstart, ylast); + if(!asknext(Yfd)) break; + t0y = (curt - t0y < 2.*G.dt) ? t0y + G.dt : curt; + } + }while(Xfd > 0 && Yfd > 0); + DBG("OOps: disconnected"); + signals(0); + return 0; +}