From f8060cfa401163d0ad9ae4ac77a6e29649d00fe1 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Tue, 18 Feb 2025 22:15:51 +0300 Subject: [PATCH] start traectories --- LibSidServo/examples/CMakeLists.txt | 1 + LibSidServo/examples/dump.c | 12 ++ LibSidServo/examples/dump.h | 1 + LibSidServo/examples/dumpmoving_scmd.c | 14 +-- LibSidServo/examples/goto.c | 30 ++++- LibSidServo/examples/scmd_traectory.c | 159 +++++++++++++++++++++++++ LibSidServo/examples/simpleconv.h | 7 +- LibSidServo/examples/traectories.c | 142 ++++++++++++++++++++++ LibSidServo/examples/traectories.h | 32 +++++ LibSidServo/libsidservo.creator.user | 2 +- LibSidServo/libsidservo.files | 3 + 11 files changed, 382 insertions(+), 21 deletions(-) create mode 100644 LibSidServo/examples/scmd_traectory.c create mode 100644 LibSidServo/examples/traectories.c create mode 100644 LibSidServo/examples/traectories.h diff --git a/LibSidServo/examples/CMakeLists.txt b/LibSidServo/examples/CMakeLists.txt index 4b286d9..bc1ebba 100644 --- a/LibSidServo/examples/CMakeLists.txt +++ b/LibSidServo/examples/CMakeLists.txt @@ -8,3 +8,4 @@ link_libraries(sidservo usefull_macros) 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(traectory_s scmd_traectory.c dump.c traectories.c) diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c index 43dd464..b31652a 100644 --- a/LibSidServo/examples/dump.c +++ b/LibSidServo/examples/dump.c @@ -129,3 +129,15 @@ int getPos(coords_t *mot, coords_t *enc){ if(enc) *enc = mdata.encposition; return TRUE; } + +// check current position and go to 0 if non-zero +void chk0(int ncycles){ + coords_t M; + if(!getPos(&M, NULL)) signals(2); + if(M.X || M.Y){ + WARNX("Mount position isn't @ zero; moving"); + Mount.moveTo(0., 0.); + waitmoving(ncycles); + green("Now mount @ zero\n"); + } +} diff --git a/LibSidServo/examples/dump.h b/LibSidServo/examples/dump.h index 3382668..414473c 100644 --- a/LibSidServo/examples/dump.h +++ b/LibSidServo/examples/dump.h @@ -26,3 +26,4 @@ void logmnt(FILE *fcoords, mountdata_t *m); void dumpmoving(FILE *fcoords, double t, int N); void waitmoving(int N); int getPos(coords_t *mot, coords_t *enc); +void chk0(int ncycles); diff --git a/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c index d3a7aaa..2adedd3 100644 --- a/LibSidServo/examples/dumpmoving_scmd.c +++ b/LibSidServo/examples/dumpmoving_scmd.c @@ -108,18 +108,6 @@ static int Wait(double tag){ return TRUE; } -// check current position and go to 0 if non-zero -static void chk0(){ - coords_t M; - if(!getPos(&M, NULL)) signals(2); - if(M.X || M.Y){ - WARNX("Mount position isn't @ zero; moving"); - Mount.moveTo(0., 0.); - waitmoving(G.Ncycles); - green("Now mount @ zero\n"); - } -} - // move X 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) @@ -163,7 +151,7 @@ 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(); + 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 diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c index 160c2cd..4d45304 100644 --- a/LibSidServo/examples/goto.c +++ b/LibSidServo/examples/goto.c @@ -30,6 +30,7 @@ typedef struct{ int help; int Ncycles; + char *coordsoutput; double X; double Y; } parameters; @@ -43,8 +44,9 @@ static parameters G = { 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 of waiting in stopped state (default: 40)"}, - {"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"}, + {"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"}, end_option }; @@ -57,15 +59,26 @@ static conf_t Config = { .SepEncoder = 0 }; +static FILE* fcoords = NULL; +static pthread_t dthr; + void signals(int sig){ + pthread_cancel(dthr); if(sig){ signal(sig, SIG_IGN); DBG("Get signal %d, quit.\n", sig); } Mount.quit(); + if(fcoords) fclose(fcoords); exit(sig); } +// dump thread +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + int main(int _U_ argc, char _U_ **argv){ sl_init(); sl_parseargs(&argc, &argv, cmdlnopts); @@ -73,11 +86,14 @@ 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"); - printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y)); - if(isnan(G.X) && isnan(G.Y)){ - Mount.quit(); - return 0; + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); } + 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); @@ -86,6 +102,8 @@ int main(int _U_ argc, char _U_ **argv){ 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(); return 0; } diff --git a/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c new file mode 100644 index 0000000..0cf7ea4 --- /dev/null +++ b/LibSidServo/examples/scmd_traectory.c @@ -0,0 +1,159 @@ +/* + * 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 "dump.h" +#include "sidservo.h" +#include "simpleconv.h" +#include "traectories.h" + +// calculate some traectory and try to run over it + +typedef struct{ + int help; + int Ncycles; // n cycles to wait stop + double reqint; // requests interval (seconds) + double Xmax; // maximal X to stop + double Ymax; // maximal Y to stop + double tmax; // maximal time of emulation + double X0; // starting point of traectory (-30..30 degr) + double Y0; // -//- + char *coordsoutput; // dump file + char *tfn; // traectory function name +} parameters; + +static FILE *fcoords = NULL; +static pthread_t dthr; +static parameters G = { + .Ncycles = 40, + .reqint = 0.1, + .tfn = "sincos", + .Xmax = 45., + .Ymax = 45., + .tmax = 300., // 5 minutes + .X0 = 10., + .Y0 = 10., +}; + +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: 40)"}, + {"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"}, + {"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1 second)"}, + {"traectory", NEED_ARG, NULL, 't', arg_string, APTR(&G.tfn), "used traectory function (default: sincos)"}, + {"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal X coordinate for traectory (default: 45 degrees)"}, + {"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal X coordinate for traectory (default: 45 degrees)"}, + {"tmax", NEED_ARG, NULL, 'T', arg_double, APTR(&G.tmax), "maximal duration time of emulation (default: 300 seconds)"}, + {"x0", NEED_ARG, NULL, '0', arg_double, APTR(&G.X0), "starting X-coordinate of traectory (default: 10 degrees)"}, + {"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"}, + end_option +}; + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + //.EncoderDevPath = "/dev/ttyUSB1", + //.EncoderDevSpeed = 153000, + .MountReqInterval = 0.05, + .SepEncoder = 0 +}; + +void signals(int sig){ + pthread_cancel(dthr); + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + if(fcoords) fclose(fcoords); + exit(sig); +} + +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 3600., G.Ncycles); + return NULL; +} + +// calculate +static void runtraectory(traectory_fn *tfn){ + if(!tfn) return; + coords_t telXY, traectXY; + double t0 = sl_dtime(); + uint32_t susec_last = 0; + while(1){ + if(!telpos(&telXY)){ + WARNX("No next telescope position"); + return; + } + if(telXY.msrtime.tv_usec == susec_last) continue; // last measure - don't mind + susec_last = telXY.msrtime.tv_usec; + double t = sl_dtime(); + if(telXY.X > G.Xmax || telXY.Y > G.Ymax || t - t0 > G.tmax) break; + if(!traectory_point(&traectXY, t)) break; + DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X), RAD2ASEC(traectXY.Y-telXY.Y)); + } + WARNX("No next traectory point"); +} + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) sl_showhelp(-1, cmdlnopts); + if(G.Xmax < 1. || G.Xmax > 90.) ERRX("Xmax should be 1..90 degrees"); + if(G.Ymax < 1. || G.Ymax > 90.) ERRX("Ymax should be 1..90 degrees"); + // convert to radians + G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax); + if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.) + ERRX("X0 and Y0 should be -30..30 degrees"); + if(G.coordsoutput){ + if(!(fcoords = fopen(G.coordsoutput, "w"))) + ERRX("Can't open %s", G.coordsoutput); + }else fcoords = stdout; + Config.MountReqInterval = G.reqint; + traectory_fn *tfn = traectory_by_name(G.tfn); + if(!tfn){ + WARNX("Bad traectory name %s, should be one of", G.tfn); + print_tr_names(); + return 1; + } + coords_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)}; + if(!init_traectory(tfn, &c)){ + ERRX("Can't init traectory"); + return 1; + } + 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 + chk0(G.Ncycles); + logmnt(fcoords, NULL); + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); + ; + runtraectory(tfn); + signals(0); + return 0; +} diff --git a/LibSidServo/examples/simpleconv.h b/LibSidServo/examples/simpleconv.h index 284b539..8200def 100644 --- a/LibSidServo/examples/simpleconv.h +++ b/LibSidServo/examples/simpleconv.h @@ -21,4 +21,9 @@ #include #define DEG2RAD(d) (d/180.*M_PI) -#define RAD2DEG(d) (d/M_PI*180.) +#define ASEC2RAD(d) (d/180.*M_PI/3600.) +#define AMIN2RAD(d) (d/180.*M_PI/60.) +#define RAD2DEG(r) (r/M_PI*180.) +#define RAD2ASEC(r) (r/M_PI*180.*3600.) +#define RAD2AMIN(r) (r/M_PI*180.*60.) + diff --git a/LibSidServo/examples/traectories.c b/LibSidServo/examples/traectories.c new file mode 100644 index 0000000..a0694e0 --- /dev/null +++ b/LibSidServo/examples/traectories.c @@ -0,0 +1,142 @@ +/* + * 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 . + */ + +// some simplest traectories +// all traectories runs increasing X and Y from starting point + +#include +#include +#include + +#include "simpleconv.h" +#include "traectories.h" + +static traectory_fn *cur_traectory = NULL; +// starting point of traectory +static coords_t XYstart = {0}; +static double tstart = 0.; +// convert Xe/Ye to approximate motor coordinates: +// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have +// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0 +static coords_t XYcor = {0}; + +/** + * @brief init_traectory - init traectory fn, sync starting positions of motor & encoders + * @param f - function calculating next point + * @param XY0 - starting point + * @return FALSE if failed + */ +int init_traectory(traectory_fn *f, coords_t *XY0){ + if(!f || !XY0) return FALSE; + cur_traectory = f; + XYstart = *XY0; + tstart = cl_dtime(); + mountdata_t mdata; + int ntries = 0; + for(; ntries < 10; ++ntries){ + if(MCC_E_OK == Mount.getMountData(&mdata)) break; + } + if(ntries == 10) return FALSE; + XYcor.X = mdata.motposition.X - mdata.encposition.X; + XYcor.Y = mdata.motposition.X + mdata.encposition.Y; + DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X), DEG2RAD(XYcor.Y)); + return TRUE; +} + +/** + * @brief traectory_point - get traectory point for given time + * @param nextpt (o) - next point coordinates + * @param t - UNIX-time of event + * @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180) + */ +int traectory_point(coords_t *nextpt, double t){ + if(t < 0. || !cur_traectory) return FALSE; + coords_t pt; + if(!cur_traectory(&pt, t)) return FALSE; + 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; + return TRUE; +} + +// current telescope position according to starting motor coordinates +// @return FALSE if failed to get current coordinates +int telpos(coords_t *curpos){ + mountdata_t mdata; + int ntries = 0; + for(; ntries < 10; ++ntries){ + if(MCC_E_OK == Mount.getMountData(&mdata)) break; + } + if(ntries == 10) return FALSE; + coords_t pt; + pt.X = XYcor.X + mdata.encposition.X; + pt.Y = XYcor.Y + mdata.encposition.Y; + pt.msrtime = mdata.encposition.msrtime; + if(curpos) *curpos = pt; + return TRUE; +} + +// X=X0+1'/s, Y=Y0+15''/s +int Linear(coords_t *nextpt, double t){ + coords_t pt; + pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart); + pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart); + if(nextpt) *nextpt = pt; + return TRUE; +} + +// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi) +int SinCos(coords_t *nextpt, double t){ + coords_t pt; + pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI); + pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI); + if(nextpt) *nextpt = pt; + return TRUE; +} + +typedef struct{ + traectory_fn *f; + const char *name; + const char *help; +} tr_names; + +static tr_names names[] = { + {Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"}, + {SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"}, + {NULL, NULL, 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){ + f = names[i].f; + break; + } + } +} + +// print all acceptable traectories names with help +void print_tr_names(){ + for(int i = 0; ; ++i){ + if(!names[i].f) break; + printf("%s: %s\n", names[i].name, names[i].help); + } +} diff --git a/LibSidServo/examples/traectories.h b/LibSidServo/examples/traectories.h new file mode 100644 index 0000000..da0f715 --- /dev/null +++ b/LibSidServo/examples/traectories.h @@ -0,0 +1,32 @@ +/* + * 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 . + */ + +#pragma once + +#include "sidservo.h" + +// 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); +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); diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 8a4701c..779b995 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 cf37c9f..27e64d2 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -5,11 +5,14 @@ examples/dump.h examples/dumpmoving.c examples/dumpmoving_scmd.c examples/goto.c +examples/scmd_traectory.c examples/simpleconv.h main.c sidservo.h serial.c examples/CMakeLists.txt +examples/traectories.c +examples/traectories.h serial.h ssii.c ssii.h