From d7df8e5bf13dca1203f817fa8abb34da8351ff55 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Sat, 8 Feb 2025 19:48:44 +0300 Subject: [PATCH] nxt --- LibSidServo/examples/dumpmoving.c | 80 +++++++++++++++-- LibSidServo/libsidservo.creator.user | 2 +- LibSidServo/main.c | 31 ++++++- LibSidServo/serial.c | 117 ++++++++++++++++-------- LibSidServo/serial.h | 6 +- LibSidServo/sidservo.h | 12 +-- LibSidServo/ssii.c | 130 ++++++++++++++++++++++++--- LibSidServo/ssii.h | 101 +++++++++++++++------ 8 files changed, 387 insertions(+), 92 deletions(-) diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index 9ad241c..6759238 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/LibSidServo/examples/dumpmoving.c @@ -16,6 +16,7 @@ * along with this program. If not, see . */ +#include #include #include #include @@ -31,6 +32,7 @@ typedef struct{ } parameters; static parameters G = {0}; +static FILE *fcoords = NULL; static sl_option_t cmdlnopts[] = { {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, @@ -51,13 +53,67 @@ void signals(int sig){ } static conf_t Config = { - .MountDevPath = "/dev/ttyS1", + .MountDevPath = "/dev/ttyUSB0", .MountDevSpeed = 19200, - //.EncoderDevPath = "/dev/ttyUSB0", + //.EncoderDevPath = "/dev/ttyUSB1", //.EncoderDevSpeed = 153000, + .MountReqInterval = 0.1, .SepEncoder = 0 }; +#define DEG2RAD(d) (d/180.*M_PI) +#define RAD2DEG(d) (d/M_PI*180.) + + +static void logmnt(mountdata_t *m){ + DBG("LOG"); + static double t0 = -1.; + if(!fcoords) return; + if(!m){ // write header + fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n"); + return; + } + if(t0 < 0.) t0 = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6; + double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6 - t0; + // write data + fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10u %6.1f %4.1f\n", + t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y), + RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y), + m->millis, m->temperature, m->voltage); +} + +// dump moving but not longer than t secs; exit after 5 stopped events +static void dumpmoving(double t){ + mountdata_t mdata; + DBG("Start dump"); + int ntries = 0; + for(; ntries < 10; ++ntries){ + if(MCC_E_OK == Mount.getMountData(&mdata)) break; + } + if(ntries == 10){ + WARNX("Can't get mount data"); + LOGWARN("Can't get mount data"); + } + suseconds_t usec = mdata.motposition.msrtime.tv_usec; + int ctr = 0; + double xlast = mdata.motposition.X, ylast = mdata.motposition.Y; + double t0 = sl_dtime(); + DBG("usec = %ld", usec); + while(sl_dtime() - t0 < t && ctr < 5){ + usleep(10000); + if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} + if(mdata.motposition.msrtime.tv_usec == usec) continue; + DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y); + usec = mdata.motposition.msrtime.tv_usec; + if(fcoords) logmnt(&mdata); + if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ + xlast = mdata.motposition.X; + ylast = mdata.motposition.Y; + ctr = 0; + }else ++ctr; + } +} + int main(int argc, char **argv){ sl_init(); sl_parseargs(&argc, &argv, cmdlnopts); @@ -65,18 +121,30 @@ int main(int argc, char **argv){ sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR; if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1; if(G.logfile) OPENLOG(G.logfile, lvl, 1); + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + logmnt(NULL); + } time_t curtime = time(NULL); LOGMSG("Started @ %s", ctime(&curtime)); - DBG("Devices ready"); LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed); LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed); + mcc_errcodes_t e = Mount.init(&Config); + if(e != MCC_E_OK){ + WARNX("Can't init devices"); + return 1; + } signal(SIGTERM, signals); // kill (-15) - quit signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGINT, signals); // ctrl+C - quit signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z - double t0 = sl_dtime(); - while(sl_dtime() - t0 < 10.); - Mount.quit(); + if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.))) + ERRX("Can't move to 45, 45"); + dumpmoving(30.); + Mount.moveTo(0., 0.); + dumpmoving(30.); + signals(0); return 0; } diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 481cf10..6016c1c 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 414b22b..450efb7 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -21,6 +21,7 @@ #include "dbg.h" #include "serial.h" +#include "ssii.h" conf_t Conf = {0}; @@ -29,6 +30,7 @@ conf_t Conf = {0}; */ static void quit(){ DBG("Close serial devices"); + for(int i = 0; i < 10; ++i) if(SSemergStop()) break; closeSerial(); DBG("Exit"); } @@ -39,6 +41,7 @@ static void quit(){ * @return error code */ static mcc_errcodes_t init(conf_t *c){ + FNAME(); if(!c) return MCC_E_BADFORMAT; Conf = *c; mcc_errcodes_t ret = MCC_E_OK; @@ -58,17 +61,39 @@ static mcc_errcodes_t init(conf_t *c){ ret = MCC_E_ENCODERDEV; } } - if(Conf.MountReqInterval < 1 || Conf.MountReqInterval > 1000000){ + if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.001){ DBG("Bad value of MountReqInterval"); - retr = MCC_E_FATAL; + ret = MCC_E_BADFORMAT; } if(ret != MCC_E_OK) quit(); return ret; } +/** + * @brief move2 - simple move to given point and stop + * @param X - new X coordinate (radians: -pi..pi) + * @param Y - new Y coordinate (radians: -pi..pi) + * @return error code + */ +static mcc_errcodes_t move2(double X, double Y){ + if(X > M_PI || X < -M_PI || Y > M_PI || Y < -M_PI){ + DBG("Wrong coords: X=%g, Y=%g", X, Y); + return MCC_E_BADFORMAT; + } + if(!SSXmoveto(X) || !SSYmoveto(Y)) return MCC_E_FAILED; + return MCC_E_OK; +} + +static mcc_errcodes_t emstop(){ + if(!SSemergStop()) return MCC_E_FAILED; + return MCC_E_OK; +} + // init mount class mount_t Mount = { .init = init, .quit = quit, - .getMountData = getMD + .getMountData = getMD, + .moveTo = move2, + .emergStop = emstop, }; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 21b307a..c1fa9ac 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -54,15 +54,11 @@ typedef struct __attribute__((packed)){ uint8_t CRC[4]; } enc_t; -#define Xpos2rad(pos) (2. * (double)pos / ENC_TURN_XTICKS * M_PI) -#define Ypos2rad(pos) (2. * (double)pos / ENC_TURN_YTICKS * M_PI) - -#if 0 /** * @brief dtime - UNIX time with microsecond * @return value */ -static double dtime(){ +double dtime(){ double t; struct timeval tv; gettimeofday(&tv, NULL); @@ -70,6 +66,7 @@ static double dtime(){ return t; } +#if 0 // init start time static void gttime(){ struct timeval tv; @@ -112,11 +109,11 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){ return; } pthread_mutex_lock(&datamutex); - mountdata.position.X = Xpos2rad(edata->encX); - mountdata.position.Y = Ypos2rad(edata->encY); - mountdata.position.msrtime = *tv; + mountdata.encposition.X = X_ENC2RAD(edata->encX); + mountdata.encposition.Y = Y_ENC2RAD(edata->encY); + mountdata.encposition.msrtime = *tv; pthread_mutex_unlock(&datamutex); - DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.position.X*180./M_PI, mountdata.position.Y*180./M_PI); + 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); } // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected @@ -149,18 +146,25 @@ static int getmntbyte(){ uint8_t byte; fd_set rfds; struct timeval tv; + /* ssize_t l = read(mntfd, &byte, 1); + //DBG("MNT read=%zd byte=0x%X", l, byte); + if(l == 0) return -1; + if(l != 1) return -2; // disconnected ?? + return (int) byte;*/ do{ FD_ZERO(&rfds); FD_SET(mntfd, &rfds); tv = mntRtmout; int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv); - if(!retval) break; if(retval < 0){ if(errno == EINTR) continue; + DBG("Error in select()"); return -1; } + //DBG("FD_ISSET = %d", FD_ISSET(mntfd, &rfds)); if(FD_ISSET(mntfd, &rfds)){ ssize_t l = read(mntfd, &byte, 1); + //DBG("MNT read=%zd byte=0x%X", l, byte); if(l != 1) return -2; // disconnected ?? break; } else return -1; @@ -199,35 +203,58 @@ static void *encoderthread(void _U_ *u){ return NULL; } +data_t *cmd2dat(const char *cmd){ + if(!cmd) return NULL; + data_t *d = calloc(1, sizeof(data_t)); + if(!d) return NULL; + d->buf = (uint8_t*)strdup(cmd); + d->len = strlen(cmd); + d->maxlen = d->len + 1; + return d; +} +void data_free(data_t **x){ + if(!x || !*x) return; + free((*x)->buf); + free(*x); + *x = NULL; +} + // main mount thread static void *mountthread(void _U_ *u){ int errctr = 0; - SSstat status; + uint8_t buf[2*sizeof(SSstat)]; + SSstat *status = (SSstat*) buf; // data to get - data_t d = {.buf = (uint8_t*)&status, .maxlen = sizeof(SSstat)}; + data_t d = {.buf = buf, .maxlen = sizeof(buf)}; // cmd to send - const data_t cmd = {.buf = CMD_GETSTAT, .len = sizeof(CMD_GETSTAT)-1, .maxlen = sizeof(CMD_GETSTAT)-1}; + const data_t *cmd_getstat = cmd2dat(CMD_GETSTAT); + double t0 = dtime(); +/* +#ifdef EBUG + double t00 = t0; +#endif +*/ while(mntfd > -1 && errctr < MAX_ERR_CTR){ // read data to status - if(MountWriteRead(&cmd, &d) || d.len != sizeof(SSstat)){ - DBG("Can't read SSstat"); + struct timeval tgot; + if(0 != gettimeofday(&tgot, NULL)) continue; + if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){ + DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len); ++errctr; continue; } - if(SScalcChecksum((uint8_t*)status, sizeof(SSstat)-2) != status.checksum){ - DBG("BAD checksum of SSstat"); + if(SScalcChecksum(buf, sizeof(SSstat)-2) != status->checksum){ + DBG("BAD checksum of SSstat, need %d", status->checksum); ++errctr; continue; } errctr = 0; pthread_mutex_lock(&datamutex); // now change data - SSconvstat(&status, &mountdata); - if(!Conf.SepEncoder){ // fill encoder data from here, as there's no separate enc thread - ; - } - ; + SSconvstat(status, &mountdata, &tgot); pthread_mutex_unlock(&datamutex); // allow writing & getters - usleep(Conf.MountReqInterval); + //DBG("t0=%g, tnow=%g", t0-t00, dtime()-t00); + while(dtime() - t0 < Conf.MountReqInterval); + t0 = dtime(); } if(mntfd > -1){ close(mntfd); @@ -249,13 +276,13 @@ static int ttyopen(const char *path, speed_t speed){ tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl tty.c_ispeed = speed; tty.c_ospeed = speed; - tty.c_cc[VMIN] = 0; // non-canonical mode - tty.c_cc[VTIME] = 5; + //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"); if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; } // try to set exclusive - ioctl(fd, TIOCEXCL); + if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");} return fd; } @@ -266,7 +293,7 @@ int openEncoder(const char *path, int speed){ encfd = ttyopen(path, (speed_t) speed); if(encfd < 0) return FALSE; encRtmout.tv_sec = 0; - encRtmout.tv_usec = 15000000 / speed; // 1.5 bytes + encRtmout.tv_usec = 200000000 / speed; // 20 bytes if(pthread_create(&encthread, NULL, encoderthread, NULL)){ close(encfd); encfd = -1; @@ -279,11 +306,23 @@ int openEncoder(const char *path, int speed){ // return FALSE if failed int openMount(const char *path, int speed){ if(mntfd > -1) close(mntfd); + DBG("Open mount %s @ %d", path, speed); mntfd = ttyopen(path, (speed_t) speed); if(mntfd < 0) return FALSE; + DBG("mntfd=%d", mntfd); + // clear buffer + while(getmntbyte() > -1); + /*int g = write(mntfd, "XXS\r", 4); + DBG("Written %d", g); + uint8_t buf[100]; + do{ + ssize_t l = read(mntfd, buf, 100); + DBG("got %zd", l); + }while(1);*/ mntRtmout.tv_sec = 0; - mntRtmout.tv_usec = 20000000 / speed; // 2 bytes + mntRtmout.tv_usec = 500000000 / speed; // 50 bytes if(pthread_create(&mntthread, NULL, mountthread, NULL)){ + DBG("Can't create thread"); close(mntfd); mntfd = -1; return FALSE; @@ -295,16 +334,18 @@ int openMount(const char *path, int speed){ // close all opened serial devices and quit threads void closeSerial(){ if(mntfd > -1){ - DBG("Close mount"); - pthread_mutex_lock(&mntmutex); + DBG("Kill mount thread"); + if(0 == pthread_cancel(mntthread)) + pthread_join(mntthread, NULL); + DBG("close fd"); close(mntfd); mntfd = -1; - pthread_mutex_unlock(&mntmutex); } if(encfd > -1){ - DBG("Close encoder"); - pthread_cancel(encthread); - pthread_join(encthread, NULL); + DBG("Kill encoder thread"); + if(0 == pthread_cancel(encthread)) + pthread_join(encthread, NULL); + DBG("close fd"); close(encfd); encfd = -1; } @@ -326,12 +367,14 @@ mcc_errcodes_t getMD(mountdata_t *d){ * @return FALSE if failed */ int MountWriteRead(const data_t *out, data_t *in){ - if(!out && !in) return FALSE; + if((!out && !in) || mntfd < 0) return FALSE; int ret = FALSE; pthread_mutex_lock(&mntmutex); if(out){ - if(out->len != write(mntfd, out->buf, out->len)) goto ext; - write(mntfd, "\r", 1); // add EOL + if(out->len != (size_t)write(mntfd, out->buf, out->len)) goto ext; + //DBG("Send to mount %zd bytes: %s", out->len, out->buf); + int g = write(mntfd, "\r", 1); // add EOL + (void) g; } if(in){ in->len = 0; diff --git a/LibSidServo/serial.h b/LibSidServo/serial.h index 9dd2aa6..79d2166 100644 --- a/LibSidServo/serial.h +++ b/LibSidServo/serial.h @@ -26,10 +26,10 @@ #define ENC_DATALEN (13) // max error counter (when read() returns -1) #define MAX_ERR_CTR (100) -// encoder ticks per turn -#define ENC_TURN_XTICKS (111111.) -#define ENC_TURN_YTICKS (111111.) +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); void closeSerial(); diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index da3eef7..63d4a6d 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -29,6 +29,7 @@ typedef enum{ MCC_E_BADFORMAT, // wrong arguments of function MCC_E_ENCODERDEV, // encoder device error or can't open MCC_E_MOUNTDEV, // mount device error or can't open + MCC_E_FAILED, // failed to run command - protocol error } mcc_errcodes_t; typedef struct{ @@ -37,7 +38,7 @@ typedef struct{ char* EncoderDevPath; // path to encoder device int EncoderDevSpeed; // serial speed int SepEncoder; // ==1 if encoder works as separate serial device - suseconds_t MountReqInterval;// interval between subsequent mount requests (microseconds) + double MountReqInterval; // maximal interval between subsequent mount requests (seconds) ; } conf_t; @@ -65,7 +66,6 @@ typedef struct{ coords_t motposition; coords_t encposition; coords_t lastmotposition; - coords_t motspeed; uint8_t keypad; extradata_t extradata; uint32_t millis; @@ -75,9 +75,11 @@ typedef struct{ // mount class typedef struct{ - mcc_errcodes_t (*init)(conf_t *c); - void (*quit)(); - mcc_errcodes_t (*getMountData)(mountdata_t *d); + 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 (*emergStop)(); // emergency stop } mount_t; extern mount_t Mount; diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index ec52002..d5372a2 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -16,13 +16,20 @@ * along with this program. If not, see . */ +#include +#include +#include + +#include "dbg.h" #include "serial.h" #include "ssii.h" uint16_t SScalcChecksum(uint8_t *buf, int len){ uint16_t checksum = 0; - for(int i = 0; i < len; i++) + for(int i = 0; i < len; i++){ + //DBG("data[%d]=0x%X", i, *buf); checksum += *buf++; + } checksum ^= 0xFF00; // invert high byte //DBG("Checksum of %d bytes: 0x%04x", len, checksum); return checksum; @@ -30,33 +37,32 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){ // send short/long binary command static int bincmd(uint8_t *cmd, int len){ - data_t d; + static data_t *dscmd = NULL, *dlcmd = NULL; + if(!dscmd) dscmd = cmd2dat(CMD_SHORTCMD); + if(!dlcmd) dlcmd = cmd2dat(CMD_LONGCMD); if(len == sizeof(SSscmd)){ ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); DBG("Short command"); - d.buf = CMD_SHORTCMD; - d.len = sizeof(CMD_SHORTCMD) - 1; - if(!MountWriteRead(&d, NULL)) return -1; + if(!MountWriteRead(dscmd, NULL)) return -1; }else if(len == sizeof(SSlcmd)){ ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); DBG("Long command"); - d.buf = CMD_LONGCMD; - d.len = sizeof(CMD_LONGCMD) - 1; - if(!MountWriteRead(&d, NULL)) return -1; + if(!MountWriteRead(dlcmd, NULL)) return -1; }else{ return -1; } DBG("Write %d bytes and wait for ans", len); + data_t d; d.buf = cmd; d.len = d.maxlen = len; return MountWriteRead(&d, &d); } // return TRUE if OK int SScmdS(SSscmd *cmd){ - return bincmd(cmd, sizeof(SSscmd)); + return bincmd((uint8_t *)cmd, sizeof(SSscmd)); } int SScmdL(SSlcmd *cmd){ - return bincmd(cmd, sizeof(SSlcmd)); + return bincmd((uint8_t *)cmd, sizeof(SSlcmd)); } @@ -65,6 +71,106 @@ int SScmdL(SSlcmd *cmd){ * @param status (i) - just read data * @param mountdata (o) - output */ -void SSconvstat(const SSstat *status, mountdata_t *mountdata){ - ; +void SSconvstat(const SSstat *s, mountdata_t *m, struct timeval *tdat){ + if(!s || !m || !tdat) return; +/* +#ifdef EBUG + static double t0 = -1.; + if(t0 < 0.) t0 = dtime(); +#endif + DBG("Convert, t=%g", dtime()-t0); +*/ + m->motposition.X = X_MOT2RAD(s->Xmot); + m->motposition.Y = Y_MOT2RAD(s->Ymot); + m->motposition.msrtime = *tdat; + // fill encoder data from here, as there's no separate enc thread + if(!Conf.SepEncoder){ + m->encposition.X = X_ENC2RAD(s->Xenc); + m->encposition.Y = Y_ENC2RAD(s->Yenc); + m->encposition.msrtime = *tdat; + } + m->lastmotposition.X = X_MOT2RAD(s->XLast); + m->lastmotposition.Y = Y_MOT2RAD(s->YLast); + m->lastmotposition.msrtime = *tdat; + m->keypad = s->keypad; + m->extradata.ExtraBits = s->ExtraBits; + m->extradata.ain0 = s->ain0; + m->extradata.ain1 = s->ain1; + m->extradata.XBits = s->XBits; + m->extradata.YBits = s->YBits; + m->millis = s->millis; + m->voltage = (double)s->voltage / 10.; + m->temperature = ((double)s->tF - 32.) * 5. / 9.; +} + +/** + * @brief SStextcmd - send simple text command to mount and return answer + * @param cmd (i) - command to send + * @param answer (o) - answer (or NULL) + * @return + */ +int SStextcmd(const char *cmd, data_t *answer){ + if(!cmd){ + DBG("try to send empty command"); + return FALSE; + } + data_t d; + d.buf = (uint8_t*) cmd; + d.len = d.maxlen = strlen(cmd); + DBG("send %zd bytes: %s", d.len, d.buf); + return MountWriteRead(&d, answer); +} + +/** + * @brief SSgetint - send text command and return integer answer + * @param cmd (i) - command to send + * @param ans (o) - intval (INT64_MAX if error) + * @return FALSE if failed + */ +int SSgetint(const char *cmd, int64_t *ans){ + if(!cmd || !ans) return FALSE; + uint8_t buf[64]; + data_t d = {.buf = buf, .len = 0, .maxlen = 64}; + if(!SStextcmd(cmd, &d)) return FALSE; + int64_t retval = INT64_MAX; + if(d.len > 1){ + char *ptr = (char*) buf; + size_t i = 0; + for(; i < d.len; ++i){ + if(isdigit(*ptr)) break; + ++ptr; + } + if(i < d.len) retval = atol(ptr); + } + DBG("read int: %" PRIi64, retval); + *ans = retval; + 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 +// BE CAREFUL: angle isn't checking here +int SSXmoveto(double pos){ + char buf[64]; + int64_t target = X_RAD2MOT(pos); + DBG("move to angle %grad = %ld", pos, target); + snprintf(buf, 63, "%s%" PRIi64, CMD_MOTX, target); + return SStextcmd(buf, NULL); +} +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 i = 0; + for(; i < 10; ++i){ + if(!SStextcmd(CMD_EMSTOPX, NULL)) continue; + if(SStextcmd(CMD_EMSTOPY, NULL)) break; + } + if(i == 10) return FALSE; + return TRUE; } diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index df4a979..aed0ca5 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -18,10 +18,12 @@ #pragma once +#include #include #include "sidservo.h" +#if 0 // ASCII commands #define U8P(x) ((uint8_t*)x) // get binary data of all statistics @@ -50,26 +52,70 @@ #define CMD_GETYAEPR U8P("XXZ") // exit ASCII checksum mode #define CMD_EXITACM U8P("YXY0\r\xb8") +#endif + +// get binary data of all statistics +#define CMD_GETSTAT ("XXS") +// send short command +#define CMD_SHORTCMD ("XXR") +// send long command +#define CMD_LONGCMD ("YXR") +// get/set X/Y in motsteps +#define CMD_MOTX ("X") +#define CMD_MOTY ("Y") +// -//- in encoders' ticks +#define CMD_ENCX ("XZ") +#define CMD_ENCY ("YZ") +// normal stop X/Y +#define CMD_STOPX ("XN") +#define CMD_STOPY ("YN") +// emergency stop +#define CMD_EMSTOPX ("XG") +#define CMD_EMSTOPY ("YG") +// getters of motor's encoders per rev +#define CMD_GETXMEPR ("XXU") +#define CMD_GETYMEPR ("XXV") +// -//- axis encoders +#define CMD_GETXAEPR ("XXT") +#define CMD_GETYAEPR ("XXZ") +// exit ASCII checksum mode +#define CMD_EXITACM ("YXY0\r\xb8") + // timeout (seconds) of reading answer (from last symbol read) #define READTIMEOUT (0.05) -// steps per revolution; TODO: move to Conf -#define HA_MOT_STEPSPERREV (4394294) -#define DEC_MOT_STEPSPERREV (3325291) +// steps per revolution +#define X_MOT_STEPSPERREV (4394294.) +#define Y_MOT_STEPSPERREV (3325291.) -// encoder's tolerance (ticks); TODO: move to Conf -#define HAENCTOL (25) -#define DECENCTOL (25) +// 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) +#define X_RAD2MOT(r) ((uint32_t)(r / 2./M_PI * X_MOT_STEPSPERREV)) +#define Y_RAD2MOT(r) ((uint32_t)(r / 2./M_PI * Y_MOT_STEPSPERREV)) + +// encoder per revolution +#define X_ENC_STEPSPERREV (67108863.) +#define Y_ENC_STEPSPERREV (67108863.) +// encoder position to radians and back +#define X_ENC2RAD(n) (2.*M_PI * (double)n / X_ENC_STEPSPERREV) +#define Y_ENC2RAD(n) (2.*M_PI * (double)n / Y_ENC_STEPSPERREV) +#define X_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * X_ENC_STEPSPERREV)) +#define Y_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * Y_ENC_STEPSPERREV)) + +// encoder's tolerance (ticks) +#define YencTOL (25.) +#define XencTOL (25.) // all need data in one typedef struct{ // 41 bytes uint8_t ctrlAddr; // 0 a8 + controller address - int32_t DECmot; // 1 Dec/HA motor position - int32_t HAmot; // 5 - int32_t DECenc; // 9 Dec/HA encoder position - int32_t HAenc; // 13 + int32_t Xmot; // 1 Dec/HA motor position + int32_t Ymot; // 5 + int32_t Xenc; // 9 Dec/HA encoder position + int32_t Yenc; // 13 uint8_t keypad; // 17 keypad status uint8_t XBits; // 18 uint8_t YBits; // 19 @@ -79,16 +125,16 @@ typedef struct{ // 41 bytes uint32_t millis; // 25 milliseconds clock int8_t tF; // 29 temperature (degF) uint8_t voltage; // 30 input voltage *10 (RA worm phase?) - uint32_t DecLast; // 31 Alt/Dec motor location at last Alt/Dec scope encoder location change - uint32_t HALast; // 35 Az/RA motor location at last Az/RA scope encoder location change + uint32_t XLast; // 31 Alt/Dec motor location at last Alt/Dec scope encoder location change + uint32_t YLast; // 35 Az/RA motor location at last Az/RA scope encoder location change uint16_t checksum; // 39 checksum, H inverted }__attribute__((packed)) SSstat; typedef struct{ - int32_t DECmot; // 0 DEC motor position - int32_t DECspeed; // 4 DEC speed - int32_t HAmot; // 8 - int32_t HAspeed; // 12 + int32_t Xmot; // 0 DEC motor position + int32_t Xspeed; // 4 DEC speed + int32_t Ymot; // 8 + int32_t Yspeed; // 12 uint8_t xychange; // 16 change Xbits/Ybits value uint8_t XBits; // 17 uint8_t YBits; // 18 @@ -96,18 +142,23 @@ typedef struct{ } __attribute__((packed)) SSscmd; // short command typedef struct{ - int32_t DECmot; // 0 DEC motor position - int32_t DECspeed; // 4 DEC speed - int32_t HAmot; // 8 - int32_t HAspeed; // 12 - int32_t DECadder; // 16 - DEC adder - int32_t HAadder; // 20 - int32_t DECatime; // 24 DEC adder time - int32_t HAatime; // 28 + int32_t Xmot; // 0 DEC motor position + int32_t Xspeed; // 4 DEC speed + int32_t Ymot; // 8 + int32_t Yspeed; // 12 + int32_t Xadder; // 16 - DEC adder + int32_t Yadder; // 20 + int32_t Xatime; // 24 DEC adder time + int32_t Yatime; // 28 uint16_t checksum; // 32 } __attribute__((packed)) SSlcmd; // long command int SScmdS(SSscmd *cmd); int SScmdL(SSlcmd *cmd); uint16_t SScalcChecksum(uint8_t *buf, int len); -void SSconvstat(const SSstat *status, mountdata_t *mountdata); +void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *tdat); +int SStextcmd(const char *cmd, data_t *answer); +int SSgetint(const char *cmd, int64_t *ans); +int SSXmoveto(double pos); +int SSYmoveto(double pos); +int SSemergStop();