From 357a0d7e194f85b2dc04cbd6ea7c8327a6ea0705 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Wed, 19 Feb 2025 23:07:26 +0300 Subject: [PATCH] add dumpswing, fix bug with uint32 instead of int32 --- LibSidServo/examples/CMakeLists.txt | 3 +- LibSidServo/examples/dump.c | 1 + LibSidServo/examples/dumpswing.c | 172 ++++++++++++++++++++++++++ LibSidServo/examples/goto.c | 18 ++- LibSidServo/examples/scmd_traectory.c | 4 +- LibSidServo/examples/traectories.c | 15 +-- LibSidServo/examples/traectories.h | 6 +- LibSidServo/libsidservo.creator.user | 2 +- LibSidServo/libsidservo.files | 1 + LibSidServo/ssii.h | 8 +- 10 files changed, 207 insertions(+), 23 deletions(-) create mode 100644 LibSidServo/examples/dumpswing.c diff --git a/LibSidServo/examples/CMakeLists.txt b/LibSidServo/examples/CMakeLists.txt index bc1ebba..54128f0 100644 --- a/LibSidServo/examples/CMakeLists.txt +++ b/LibSidServo/examples/CMakeLists.txt @@ -2,10 +2,11 @@ project(examples) # common includes & library include_directories(../) -link_libraries(sidservo usefull_macros) +link_libraries(sidservo usefull_macros -lm) # exe list add_executable(goto goto.c dump.c) add_executable(dump dumpmoving.c dump.c) add_executable(dump_s dumpmoving_scmd.c dump.c) +add_executable(dumpswing dumpswing.c dump.c) add_executable(traectory_s scmd_traectory.c dump.c traectories.c) diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c index b31652a..4b57543 100644 --- a/LibSidServo/examples/dump.c +++ b/LibSidServo/examples/dump.c @@ -97,6 +97,7 @@ void waitmoving(int N){ usleep(10000); if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} if(mdata.millis == millis) continue; + millis = mdata.millis; if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ xlast = mdata.motposition.X; ylast = mdata.motposition.Y; diff --git a/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c new file mode 100644 index 0000000..b2bbba0 --- /dev/null +++ b/LibSidServo/examples/dumpswing.c @@ -0,0 +1,172 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 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 "dump.h" +#include "sidservo.h" +#include "simpleconv.h" + +// swing telescope by given axis with given period and max amplitude, reqinterval=0.05 (min) + +typedef struct{ + int help; + int Ncycles; + int Nswings; + double period; + double amplitude; + char *coordsoutput; + char *axis; +} parameters; + +static parameters G = { + .Ncycles = 20, + .axis = "X", + .Nswings = 10, + .period = 1., + .amplitude = 5., +}; +static FILE *fcoords = NULL; + +static sl_option_t cmdlnopts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 20)"}, + {"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"}, + {"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"}, + {"period", NEED_ARG, NULL, 'p', arg_double, APTR(&G.period), "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"}, + {"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"}, + {"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"}, + end_option +}; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + exit(sig); +} + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + //.EncoderDevPath = "/dev/ttyUSB1", + //.EncoderDevSpeed = 153000, + .MountReqInterval = 0.05, + .SepEncoder = 0 +}; + +// dump thread +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + +// wait until mount is stopped within 5 cycles or until time reached t +void waithalf(double t){ + mountdata_t mdata; + int ctr = -1; + uint32_t millis = 0; + double xlast = 0., ylast = 0.; + while(ctr < 5){ + if(sl_dtime() >= t) return; + usleep(1000); + if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} + if(mdata.millis == millis) continue; + millis = mdata.millis; + if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ + DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y)); + xlast = mdata.motposition.X; + ylast = mdata.motposition.Y; + ctr = 0; + }else{ + DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y)); + ++ctr; + } + } +} + + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) sl_showhelp(-1, cmdlnopts); + if(strcmp(G.axis, "X") && strcmp(G.axis, "Y")){ + WARNX("\"Axis\" should be X or Y"); + return 1; + } + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + if(G.Ncycles < 7) ERRX("Ncycles should be >7"); + if(G.amplitude < 0.01 || G.amplitude > 45.) + ERRX("Amplitude should be from 0.01 to 45 degrees"); + if(G.period < 0.1 || G.period > 900.) + ERRX("Period should be from 0.1 to 900s"); + if(G.Nswings < 1) ERRX("Nswings should be more than 0"); + 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 + pthread_t dthr; + chk0(G.Ncycles); + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); + G.period /= 2.; // pause between commands + double tagX, tagY; + if(*G.axis == 'X'){ + tagX = DEG2RAD(G.amplitude); tagY = 0.; + }else{ + tagX = 0.; tagY = DEG2RAD(G.amplitude); + } + double t = sl_dtime(), t0 = t; + double divide = 2.; + for(int i = 0; i < G.Nswings; ++i){ + 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); + t += G.period; + waithalf(t); + DBG("Moved to -, t=%g", t-t0); + DBG("CMD: %g", sl_dtime()-t0); + } + // be sure to move @ 0,0 + Mount.moveTo(0., 0.); + // wait moving ends + pthread_join(dthr, NULL); +#undef SCMD + signals(0); + return 0; +} diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c index 4d45304..18ebab0 100644 --- a/LibSidServo/examples/goto.c +++ b/LibSidServo/examples/goto.c @@ -30,6 +30,7 @@ typedef struct{ int help; int Ncycles; + int wait; char *coordsoutput; double X; double Y; @@ -47,6 +48,7 @@ static sl_option_t cmdlnopts[] = { {"newx", NEED_ARG, NULL, 'X', arg_double, APTR(&G.X), "new X coordinate"}, {"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"}, end_option }; @@ -86,6 +88,10 @@ int main(int _U_ argc, char _U_ **argv){ if(MCC_E_OK != Mount.init(&Config)) ERRX("Can't init mount"); coords_t M; if(!getPos(&M, NULL)) ERRX("Can't get current position"); + if(G.coordsoutput){ + if(!G.wait) green("When logging I should wait until moving ends; added '-w'"); + G.wait = 1; + } if(G.coordsoutput){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); @@ -98,12 +104,14 @@ int main(int _U_ argc, char _U_ **argv){ 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)); - sleep(1); - waitmoving(G.Ncycles); - if(!getPos(&M, NULL)) WARNX("Can't get current position"); - else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y)); + if(G.wait){ + sleep(1); + waitmoving(G.Ncycles); + if(!getPos(&M, NULL)) WARNX("Can't get current position"); + else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y)); + } out: if(G.coordsoutput) pthread_join(dthr, NULL); - Mount.quit(); + if(G.wait) Mount.quit(); return 0; } diff --git a/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c index 0cf7ea4..19ae664 100644 --- a/LibSidServo/examples/scmd_traectory.c +++ b/LibSidServo/examples/scmd_traectory.c @@ -93,7 +93,7 @@ static void *dumping(void _U_ *u){ } // calculate -static void runtraectory(traectory_fn *tfn){ +static void runtraectory(traectory_fn tfn){ if(!tfn) return; coords_t telXY, traectXY; double t0 = sl_dtime(); @@ -128,7 +128,7 @@ int main(int argc, char **argv){ ERRX("Can't open %s", G.coordsoutput); }else fcoords = stdout; Config.MountReqInterval = G.reqint; - traectory_fn *tfn = traectory_by_name(G.tfn); + traectory_fn tfn = traectory_by_name(G.tfn); if(!tfn){ WARNX("Bad traectory name %s, should be one of", G.tfn); print_tr_names(); diff --git a/LibSidServo/examples/traectories.c b/LibSidServo/examples/traectories.c index a0694e0..decd645 100644 --- a/LibSidServo/examples/traectories.c +++ b/LibSidServo/examples/traectories.c @@ -26,7 +26,7 @@ #include "simpleconv.h" #include "traectories.h" -static traectory_fn *cur_traectory = NULL; +static traectory_fn cur_traectory = NULL; // starting point of traectory static coords_t XYstart = {0}; static double tstart = 0.; @@ -41,11 +41,11 @@ static coords_t XYcor = {0}; * @param XY0 - starting point * @return FALSE if failed */ -int init_traectory(traectory_fn *f, coords_t *XY0){ +int init_traectory(traectory_fn f, coords_t *XY0){ if(!f || !XY0) return FALSE; cur_traectory = f; XYstart = *XY0; - tstart = cl_dtime(); + tstart = sl_dtime(); mountdata_t mdata; int ntries = 0; for(; ntries < 10; ++ntries){ @@ -71,7 +71,7 @@ int traectory_point(coords_t *nextpt, double t){ pt.msrtime.tv_sec = floor(t); pt.msrtime.tv_usec = (uint32_t) t - pt.msrtime.tv_sec; if(nextpt) *nextpt = pt; - if(pt.X < -M_PI2 || pt.X > M_PI2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE; + if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE; return TRUE; } @@ -111,7 +111,7 @@ int SinCos(coords_t *nextpt, double t){ } typedef struct{ - traectory_fn *f; + traectory_fn f; const char *name; const char *help; } tr_names; @@ -122,8 +122,8 @@ static tr_names names[] = { {NULL, NULL, NULL} }; -traectory_fn *traectory_by_name(const char *name){ - traectory_fn *f = NULL; +traectory_fn traectory_by_name(const char *name){ + traectory_fn f = NULL; for(int i = 0; ; ++i){ if(!names[i].f) break; if(strcmp(names[i].name, name) == 0){ @@ -131,6 +131,7 @@ traectory_fn *traectory_by_name(const char *name){ break; } } + return f; } // print all acceptable traectories names with help diff --git a/LibSidServo/examples/traectories.h b/LibSidServo/examples/traectories.h index da0f715..cfd9b2c 100644 --- a/LibSidServo/examples/traectories.h +++ b/LibSidServo/examples/traectories.h @@ -23,10 +23,10 @@ // traectory typedef int (*traectory_fn)(coords_t *, double); -int init_traectory(traectory_fn *f, coords_t *XY0); -traectory_fn *traectory_by_name(const char *name); +int init_traectory(traectory_fn f, coords_t *XY0); +traectory_fn traectory_by_name(const char *name); void print_tr_names(); int traectory_point(coords_t *nextpt, double t); int telpos(coords_t *curpos); int Linear(coords_t *nextpt, double t); -int SinCos(coords_t *nextpt); +int SinCos(coords_t *nextpt, double t); diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 779b995..524f200 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files index 27e64d2..4adc54f 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -4,6 +4,7 @@ examples/dump.c examples/dump.h examples/dumpmoving.c examples/dumpmoving_scmd.c +examples/dumpswing.c examples/goto.c examples/scmd_traectory.c examples/simpleconv.h diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index ebf8c1f..9250dd9 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -90,13 +90,13 @@ // 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)) +#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.*1953.) -#define X_RS2MOTSPD(r) ((uint32_t)(X_RAD2MOT(r)*65536./1953.)) +#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r)*65536./1953.)) #define Y_MOTSPD2RS(n) (Y_MOT2RAD(n)/65536.*1953.) -#define Y_RS2MOTSPD(r) ((uint32_t)(Y_RAD2MOT(r)*65536./1953.)) +#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953.)) // adder time to seconds vice versa #define ADDER2S(a) (a*1953.) #define S2ADDER(s) (s/1953.)