From f83f95c9ccd45f20a34fb777b8e275660a3efcbd Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Mon, 24 Feb 2025 22:23:52 +0300 Subject: [PATCH] ... --- LibSidServo/examples/Readme.md | 24 +++++++ LibSidServo/examples/dump.c | 7 +- LibSidServo/examples/dumpmoving.c | 5 +- LibSidServo/examples/dumpmoving_scmd.c | 19 ++--- LibSidServo/examples/dumpswing.c | 9 +-- LibSidServo/examples/goto.c | 22 ++++-- LibSidServo/libsidservo.creator.user | 2 +- LibSidServo/main.c | 96 +++++++++++++++++++++++--- LibSidServo/serial.c | 15 ++-- LibSidServo/sidservo.h | 5 +- LibSidServo/ssii.c | 32 ++++----- LibSidServo/ssii.h | 11 +-- 12 files changed, 186 insertions(+), 61 deletions(-) create mode 100644 LibSidServo/examples/Readme.md diff --git a/LibSidServo/examples/Readme.md b/LibSidServo/examples/Readme.md new file mode 100644 index 0000000..f4eefec --- /dev/null +++ b/LibSidServo/examples/Readme.md @@ -0,0 +1,24 @@ +Some examples of usage of libsidservo +===================================== + +## Auxiliary files + +*dump.c*, *dump.h* - base logging and dumping functions, also some useful functions like get current position and move to zero if current position isn't at zero. + +*traectories.c*, *traectories.h* - modeling simple moving object traectories; also some functions like get current position in encoders' angles setting to zero at motors' zero. + +*simpleconv.h* + + +## Examples + +*dumpmoving.c* (`dump`) - dump moving relative starting point by simplest text commands "X" and "Y". + +*dumpmoving_scmd.c* (`dump_s`) - moving relative starting point using "short" binary command. + +*dumpswing.c* (`dumpswing`) - shake telescope around starting point by one of axis. + +*goto.c* (`goto`) - get current coordinates or go to given (by simplest "X/Y" commands). + +*scmd_traectory.c* (`traectory_s`) - try to move around given traectory using "short" binary commands. + diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c index 4b57543..4b0c581 100644 --- a/LibSidServo/examples/dump.c +++ b/LibSidServo/examples/dump.c @@ -107,13 +107,13 @@ void waitmoving(int N){ } /** - * @brief getMotPos - get current + * @brief getPos - get current position * @param mot (o) - motor position (or NULL) * @param Y (o) - encoder position (or NULL) * @return FALSE if failed */ int getPos(coords_t *mot, coords_t *enc){ - mountdata_t mdata; + mountdata_t mdata = {0}; int errcnt = 0; do{ if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt; @@ -137,7 +137,8 @@ void chk0(int ncycles){ if(!getPos(&M, NULL)) signals(2); if(M.X || M.Y){ WARNX("Mount position isn't @ zero; moving"); - Mount.moveTo(0., 0.); + double zero = 0.; + Mount.moveTo(&zero, &zero); waitmoving(ncycles); green("Now mount @ zero\n"); } diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index e08b80e..2769529 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/LibSidServo/examples/dumpmoving.c @@ -95,10 +95,11 @@ int main(int argc, char **argv){ signal(SIGINT, signals); // ctrl+C - quit signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z - if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.))) + double tag = DEG2RAD(45.), zero = 0.; + if(MCC_E_OK != Mount.moveTo(&tag, &tag)) ERRX("Can't move to 45, 45"); dumpmoving(fcoords, 30., G.Ncycles); - Mount.moveTo(0., 0.); + Mount.moveTo(&zero, &zero); dumpmoving(fcoords, 30., G.Ncycles); signals(0); return 0; diff --git a/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c index 2adedd3..a3f23bc 100644 --- a/LibSidServo/examples/dumpmoving_scmd.c +++ b/LibSidServo/examples/dumpmoving_scmd.c @@ -72,6 +72,8 @@ static conf_t Config = { .SepEncoder = 0 }; +static coords_t M; + // dump thread static void *dumping(void _U_ *u){ dumpmoving(fcoords, 3600., G.Ncycles); @@ -108,20 +110,22 @@ static int Wait(double tag){ return TRUE; } -// move X to 40 degr with given speed until given coord +// move X/Y to 40 degr with given speed until given coord static void move(double target, double limit, double speed){ #define SCMD() do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0) green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed); short_command_t cmd = {0}; if(*G.axis == 'X'){ - cmd.Xmot = DEG2RAD(target); + cmd.Xmot = DEG2RAD(target) + M.X; cmd.Xspeed = DEG2RAD(speed); + limit = DEG2RAD(limit) + M.X; }else{ - cmd.Ymot = DEG2RAD(target); + cmd.Ymot = DEG2RAD(target) + M.Y; cmd.Yspeed = DEG2RAD(speed); + limit = DEG2RAD(limit) + M.Y; } SCMD(); - if(!Wait(DEG2RAD(limit))) signals(9); + if(!Wait(limit)) signals(9); #undef SCMD } @@ -139,11 +143,11 @@ int main(int argc, char **argv){ ERRX("Can't open %s", G.coordsoutput); }else fcoords = stdout; Config.MountReqInterval = G.reqint; - mcc_errcodes_t e = Mount.init(&Config); - if(e != MCC_E_OK){ + if(MCC_E_OK != Mount.init(&Config)){ WARNX("Can't init devices"); return 1; } + if(!getPos(&M, NULL)) ERRX("Can't get current position"); signal(SIGTERM, signals); // kill (-15) - quit signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGINT, signals); // ctrl+C - quit @@ -151,7 +155,6 @@ int main(int argc, char **argv){ signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z // move to X=40 degr with different speeds pthread_t dthr; - chk0(G.Ncycles); logmnt(fcoords, NULL); if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); // goto 1 degr with 1'/s @@ -165,7 +168,7 @@ int main(int argc, char **argv){ // and go back with 5deg/s move(0., 0., 5.); // be sure to move @ 0,0 - Mount.moveTo(0., 0.); + Mount.moveTo(&M.X, &M.Y); // wait moving ends pthread_join(dthr, NULL); #undef SCMD diff --git a/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c index b2bbba0..f1d7ca0 100644 --- a/LibSidServo/examples/dumpswing.c +++ b/LibSidServo/examples/dumpswing.c @@ -147,23 +147,24 @@ int main(int argc, char **argv){ tagX = 0.; tagY = DEG2RAD(G.amplitude); } double t = sl_dtime(), t0 = t; - double divide = 2.; + double divide = 2., rtagX = -tagX, rtagY = -tagY; for(int i = 0; i < G.Nswings; ++i){ - Mount.moveTo(tagX, tagY); + Mount.moveTo(&tagX, &tagY); DBG("CMD: %g", sl_dtime()-t0); t += G.period / divide; divide = 1.; waithalf(t); DBG("Moved to +, t=%g", t-t0); DBG("CMD: %g", sl_dtime()-t0); - Mount.moveTo(-tagX, -tagY); + Mount.moveTo(&rtagX, &rtagY); t += G.period; waithalf(t); DBG("Moved to -, t=%g", t-t0); DBG("CMD: %g", sl_dtime()-t0); } + double zero = 0.; // be sure to move @ 0,0 - Mount.moveTo(0., 0.); + Mount.moveTo(&zero, &zero); // wait moving ends pthread_join(dthr, NULL); #undef SCMD diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c index 18ebab0..0864adf 100644 --- a/LibSidServo/examples/goto.c +++ b/LibSidServo/examples/goto.c @@ -31,6 +31,7 @@ typedef struct{ int help; int Ncycles; int wait; + int relative; char *coordsoutput; double X; double Y; @@ -49,6 +50,7 @@ static sl_option_t cmdlnopts[] = { {"newy", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Y), "new Y coordinate"}, {"output", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"file to log coordinates"}, {"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), "wait until mowing stopped"}, + {"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"}, end_option }; @@ -100,10 +102,22 @@ 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; - if(isnan(G.X)) G.X = RAD2DEG(M.X); - if(isnan(G.Y)) G.Y = RAD2DEG(M.Y); - printf("Moving to X=%g deg, Y=%g deg\n", G.X, G.Y); - Mount.moveTo(DEG2RAD(G.X), DEG2RAD(G.Y)); + double *xtag = NULL, *ytag = NULL, xr, yr; + if(!isnan(G.X)){ + xr = DEG2RAD(G.X); + if(G.relative) xr += M.X; + xtag = &xr; + } + if(!isnan(G.Y)){ + yr = DEG2RAD(G.Y); + if(G.relative) yr += M.Y; + ytag = &yr; + } + printf("Moving to "); + if(xtag) printf("X=%gdeg ", G.X); + if(ytag) printf("Y=%gdeg", G.Y); + printf("\n"); + Mount.moveTo(xtag, ytag); if(G.wait){ sleep(1); waitmoving(G.Ncycles); diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index db02460..6cedb0a 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 f7d3592..a5fdf27 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -16,6 +16,7 @@ * along with this program. If not, see . */ +#include #include #include @@ -30,7 +31,7 @@ conf_t Conf = {0}; */ static void quit(){ DBG("Close serial devices"); - for(int i = 0; i < 10; ++i) if(SSemergStop()) break; + for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break; closeSerial(); DBG("Exit"); } @@ -73,18 +74,87 @@ static mcc_errcodes_t init(conf_t *c){ return ret; } +// check coordinates and speeds; return FALSE if failed +// TODO fix to real limits!!! +static int chkX(double X){ + if(X > 2.*M_PI || X < -2.*M_PI) return FALSE; + return TRUE; +} +static int chkY(double Y){ + if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE; + return TRUE; +} +static int chkXs(double s){ + if(s < 0. || s > X_SPEED_MAX) return FALSE; + return TRUE; +} +static int chkYs(double s){ + if(s < 0. || s > Y_SPEED_MAX) return FALSE; + return TRUE; +} + + /** * @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) + * @param X - new X coordinate (radians: -pi..pi) or NULL + * @param Y - new Y coordinate (radians: -pi..pi) or NULL * @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; +static mcc_errcodes_t move2(const double *X, const double *Y){ + if(!X && !Y) return MCC_E_BADFORMAT; + if(X){ + if(!chkX(*X)) return MCC_E_BADFORMAT; + int64_t tag = X_RAD2MOT(*X); + if(!SSsetterI(CMD_MOTX, tag)) return MCC_E_FAILED; } - if(!SSXmoveto(X) || !SSYmoveto(Y)) return MCC_E_FAILED; + if(Y){ + if(!chkY(*Y)) return MCC_E_BADFORMAT; + int64_t tag = X_RAD2MOT(*Y); + if(!SSsetterI(CMD_MOTY, tag)) return MCC_E_FAILED; + } + return MCC_E_OK; +} + +/** + * @brief setspeed - set maximal speed over axis + * @param X (i) - max speed or NULL + * @param Y (i) - -//- + * @return errcode + */ +static mcc_errcodes_t setspeed(const double *X, const double *Y){ + if(!X && !Y) return MCC_E_BADFORMAT; + if(X){ + if(!chkXs(*X)) return MCC_E_BADFORMAT; + int32_t spd = X_RS2MOTSPD(*X); + if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED; + } + if(Y){ + if(!chkYs(*Y)) return MCC_E_BADFORMAT; + int32_t spd = Y_RS2MOTSPD(*Y); + if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED; + } + return MCC_E_OK; +} + +/** + * @brief move2s - move to target with given max speed + * @param target (i) - target or NULL + * @param speed (i) - speed or NULL + * @return + */ +static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){ + if(!target && !speed) return MCC_E_BADFORMAT; + if(!target) return setspeed(&speed->X, &speed->Y); + if(!speed) return move2(&target->X, &target->Y); + if(!chkX(target->X) || !chkY(target->Y) || !chkXs(speed->X) || !chkYs(speed->Y)) + 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); + 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); + if(!SStextcmd(buf, NULL)) return MCC_E_FAILED; return MCC_E_OK; } @@ -93,7 +163,12 @@ static mcc_errcodes_t move2(double X, double Y){ * @return errcode */ static mcc_errcodes_t emstop(){ - if(!SSemergStop()) return MCC_E_FAILED; + if(!SSstop(TRUE)) return MCC_E_FAILED; + return MCC_E_OK; +} +// normal stop +static mcc_errcodes_t stop(){ + if(!SSstop(FALSE)) return MCC_E_FAILED; return MCC_E_OK; } @@ -159,7 +234,10 @@ mount_t Mount = { .quit = quit, .getMountData = getMD, .moveTo = move2, + .moveWspeed = move2s, + .setSpeed = setspeed, .emergStop = emstop, + .stop = stop, .shortCmd = shortcmd, .longCmd = longcmd, }; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 11a36e0..344be02 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -375,13 +375,14 @@ static int wr(const data_t *out, data_t *in, int needeol){ (void) g; } } - if(in){ - in->len = 0; - for(size_t i = 0; i < in->maxlen; ++i){ - int b = getmntbyte(); - if(b < 0) break; // nothing to read -> go out - in->buf[in->len++] = (uint8_t) b; - } + uint8_t buf[256]; + data_t dumb = {.buf = buf, .maxlen = 256}; + if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash + in->len = 0; + for(size_t i = 0; i < in->maxlen; ++i){ + int b = getmntbyte(); + if(b < 0) break; // nothing to read -> go out + in->buf[in->len++] = (uint8_t) b; } return TRUE; } diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 148daa8..1f895a9 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -130,7 +130,10 @@ typedef struct{ 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 (*moveTo)(const double *X, const double *Y); // move to given position ans stop + mcc_errcodes_t (*moveWspeed)(const coords_t *target, const coords_t *speed); // move with given max speed + mcc_errcodes_t (*setSpeed)(const double *X, const double *Y); // set speed + mcc_errcodes_t (*stop)(); // stop mcc_errcodes_t (*emergStop)(); // emergency stop mcc_errcodes_t (*shortCmd)(short_command_t *cmd); // send/get short command mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index 8ec8f00..5f1594e 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -129,29 +129,25 @@ int SSgetint(const char *cmd, int64_t *ans){ 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); +/** + * @brief SSsetterI - integer setter + * @param cmd - command to send + * @param ival - value + * @return false if failed + */ +int SSsetterI(const char *cmd, int32_t ival){ + char buf[128]; + snprintf(buf, 127, "%s%" PRIi64, cmd, ival); return SStextcmd(buf, NULL); } -int SSemergStop(){ +int SSstop(int emerg){ int i = 0; + const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX; + const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY; for(; i < 10; ++i){ - if(!SStextcmd(CMD_EMSTOPX, NULL)) continue; - if(SStextcmd(CMD_EMSTOPY, NULL)) break; + if(!SStextcmd(cmdx, NULL)) continue; + if(SStextcmd(cmdy, NULL)) break; } if(i == 10) return FALSE; return TRUE; diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index a6316cb..7911bf6 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -28,7 +28,7 @@ #define CMD_MOTX "X" #define CMD_MOTY "Y" // set X/Y position with speed "sprintf(buf, "%s%d%s%d", CMD_MOTx, tagx, CMD_MOTxS, tags) -#define CMD_MOTXS "S" +#define CMD_MOTXYS "S" // reset current motor position to given value (and stop, if moving) #define CMD_MOTXSET "XF" #define CMD_MOTYSET "YF" @@ -173,6 +173,10 @@ //#define Y_MOT_STEPSPERREV (4394496.) #define Y_MOT_STEPSPERREV (4394960.) +// maximal speeds in rad/s: 10deg/s by X and 8deg/s by Y +#define X_SPEED_MAX (0.17453) +#define Y_SPEED_MAX (0.13963) + // 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) @@ -250,7 +254,6 @@ void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *td int SStextcmd(const char *cmd, data_t *answer); int SSrawcmd(const char *cmd, data_t *answer); int SSgetint(const char *cmd, int64_t *ans); -int SSXmoveto(double pos); -int SSYmoveto(double pos); -int SSemergStop(); +int SSsetterI(const char *cmd, int32_t ival); +int SSstop(int emerg); int SSshortCmd(SSscmd *cmd);