From d3dd334aa74490fa4f86b0455df37e98b7e132b2 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Sun, 31 Jul 2022 21:22:11 +0300 Subject: [PATCH] SSII --- Auxiliary_utils/SiderealServoII/Makefile | 2 +- Auxiliary_utils/SiderealServoII/cmdlnopts.c | 5 +- Auxiliary_utils/SiderealServoII/cmdlnopts.h | 1 + Auxiliary_utils/SiderealServoII/emulator.c | 115 ++++++ Auxiliary_utils/SiderealServoII/emulator.h | 35 ++ Auxiliary_utils/SiderealServoII/main.c | 87 +++-- Auxiliary_utils/SiderealServoII/mot.log | 210 +++++++++++ Auxiliary_utils/SiderealServoII/motlog.c | 69 ++++ Auxiliary_utils/SiderealServoII/motlog.h | 25 ++ Auxiliary_utils/SiderealServoII/msg.log | 3 + Auxiliary_utils/SiderealServoII/sidservo.c | 376 ++++++++++++++++++-- Auxiliary_utils/SiderealServoII/sidservo.h | 110 ++++-- 12 files changed, 946 insertions(+), 92 deletions(-) create mode 100644 Auxiliary_utils/SiderealServoII/emulator.c create mode 100644 Auxiliary_utils/SiderealServoII/emulator.h create mode 100644 Auxiliary_utils/SiderealServoII/mot.log create mode 100644 Auxiliary_utils/SiderealServoII/motlog.c create mode 100644 Auxiliary_utils/SiderealServoII/motlog.h create mode 100644 Auxiliary_utils/SiderealServoII/msg.log diff --git a/Auxiliary_utils/SiderealServoII/Makefile b/Auxiliary_utils/SiderealServoII/Makefile index 8943f93..3ee063d 100644 --- a/Auxiliary_utils/SiderealServoII/Makefile +++ b/Auxiliary_utils/SiderealServoII/Makefile @@ -1,7 +1,7 @@ # run `make DEF=...` to add extra defines PROGRAM := runscope LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all -LDFLAGS += -lusefull_macros +LDFLAGS += -lusefull_macros -lm SRCS := $(wildcard *.c) DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111 OBJDIR := mk diff --git a/Auxiliary_utils/SiderealServoII/cmdlnopts.c b/Auxiliary_utils/SiderealServoII/cmdlnopts.c index 90c32a3..ed6da76 100644 --- a/Auxiliary_utils/SiderealServoII/cmdlnopts.c +++ b/Auxiliary_utils/SiderealServoII/cmdlnopts.c @@ -50,9 +50,10 @@ static glob_pars const Gdefault = { static myoption cmdlnopts[] = { // common options {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&help), _("show this help")}, - {"device", NEED_ARG, NULL, 'd', arg_string, APTR(&G.device), _("serial device name (default:" DEFAULT_SERDEV ")")}, - {"speed", NEED_ARG, NULL, 's', arg_int, APTR(&G.speed), _("serial device speed (default: 115200)")}, + {"device", NEED_ARG, NULL, 'd', arg_string, APTR(&G.device), _("serial device name (default: " DEFAULT_SERDEV ")")}, + {"speed", NEED_ARG, NULL, 's', arg_int, APTR(&G.speed), _("serial device speed (default: 19200)")}, {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), _("file to save logs")}, + {"motlog", NEED_ARG, NULL, 'm', arg_string, APTR(&G.motorslog), _("log motors' data into this file")}, {"pidfile", NEED_ARG, NULL, 'P', arg_string, APTR(&G.pidfile), _("pidfile (default: " DEFAULT_PIDFILE ")")}, end_option }; diff --git a/Auxiliary_utils/SiderealServoII/cmdlnopts.h b/Auxiliary_utils/SiderealServoII/cmdlnopts.h index 59c14cb..45b01d9 100644 --- a/Auxiliary_utils/SiderealServoII/cmdlnopts.h +++ b/Auxiliary_utils/SiderealServoII/cmdlnopts.h @@ -27,6 +27,7 @@ typedef struct{ char *device; // serial device name char *pidfile; // name of PID file char *logfile; // logging to this file + char *motorslog; // log of motors' data int speed; // connection speed int rest_pars_num; // number of rest parameters char** rest_pars; // the rest parameters: array of char* diff --git a/Auxiliary_utils/SiderealServoII/emulator.c b/Auxiliary_utils/SiderealServoII/emulator.c new file mode 100644 index 0000000..51eef8f --- /dev/null +++ b/Auxiliary_utils/SiderealServoII/emulator.c @@ -0,0 +1,115 @@ +/* + * This file is part of the SSII project. + * Copyright 2022 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 "emulator.h" +#include "motlog.h" + +// HA/DEC of starting point, starting time +static double Eha0 = 0., Edec0 = 0., Et0 = 0.; + +// monitor each 100ms motor's motion; stop when coords are the same for 3 times in a row +// inistat - returning of binary command (if len equal to sizeof SSstat) +void SSmotor_monitoring(SSstat *inistat){ + DBG("Start monitoring"); + //int32_t oldDec = 0, oldRA = 0; // old positions in encoder values (also for speed calculations) + SSstat prevstat = {0}; + uint8_t start = 1, repeat = 0, errctr = 0; + int32_t oldRAm = 0, oldDm = 0; + double tlast = dtime(); + if(inistat){ // get initial values + start = 0; + prevstat = *inistat; + oldRAm = prevstat.HAmot; + oldDm = prevstat.DECmot; + } + while(1){ + if(start){ // didn't get initial values -> need to know first + if(!SSgetPartialstat(&prevstat)) continue; + start = 0; + tlast = dtime(); + oldRAm = prevstat.HAmot; + oldDm = prevstat.DECmot; + }else{ + if(!SSlog_motor_data(&prevstat, &tlast)){ + WARNX("Hmmm... in %dth time", ++errctr); + if(++errctr > 10) break; + else continue; + }else errctr = 0; + } + if(prevstat.HAmot == oldRAm && prevstat.DECmot == oldDm){ + if(++repeat > 2) break; + }else repeat = 0; + oldRAm = prevstat.HAmot; + oldDm = prevstat.DECmot; + } + DBG("End of monitoring"); +} + + +/** + * @brief log_motor_data - get current config & put to log difference between values + * @param old (io) - old parameters value: as it was -> as it become + * @param told (io) - previous time (`dtime()` output) -> current time + * @return TRUE if all OK (if return FALSE, the `old` and `told` won't be changed! + */ +int SSlog_motor_data(SSstat *old, double *told){ + SSstat s; + if(!old || !told) return FALSE; + if(!SSgetPartialstat(&s)) return FALSE; + double tnow = dtime(), tdif = tnow - *told; + mot_log(1, "%d\t%d\t%.1f\t%d\t%d\t%.1f", s.DECmot, s.DECenc, (s.DECmot - old->DECmot) / tdif, + s.HAmot, s.HAenc, (s.HAmot - old->HAmot) / tdif); + *old = s; + *told = tnow; + return TRUE; +} + +// calculate next `target` position in current moment of time +// ha changed as 15''/s + 15*sin(t*2pi/600s); +// dec changed as 30*cos(t*2pi/600s) +static void emul_next_pos(double *ha, double *dec, double tnew){ + if(!ha || !dec) return; + double s1, c1, t = tnew - Et0; + sincos(t*2.*M_PI/600., &s1, &c1); + *ha = Eha0 + ARCSEC2DEG(15) * (t + s1); + *dec = Edec0 + ARCSEC2DEG(30) * c1; +} + +// init emulated variables and start target's searching +void SSstart_emulation(double ha_start, double dec_start){ + Eha0 = ha_start; + Edec0 = dec_start; + Et0 = dtime(); + DBG("Try to send short command"); + SSscmd sc = {0}; + // now calculate first starting position and try to catch it + double ha, dec; + emul_next_pos(&ha, &dec, Et0 + 1.); + mot_log(0, "goto ha=%d, dec=%d"); + SSgoto(ha, dec); + SSwaitmoving(); + // now we are near of the target: start to catch it + ; + while(SScmds(&sc) != sizeof(SSstat)) WARNX("SSCMDshort bad answer!"); + +} + diff --git a/Auxiliary_utils/SiderealServoII/emulator.h b/Auxiliary_utils/SiderealServoII/emulator.h new file mode 100644 index 0000000..b9483a2 --- /dev/null +++ b/Auxiliary_utils/SiderealServoII/emulator.h @@ -0,0 +1,35 @@ +/* + * This file is part of the SSII project. + * Copyright 2022 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" + +#define SECPERRAD (206264.802) + +// convert angles +#define ARCSEC2DEG(x) (x/3600.) +#define DEG2ARCSEC(x) (x*3600.) +#define ARCSEC2RAD(x) (x/SECPERRAD) +#define RAD2ARCSEC(x) (x*SECPERRAD) + + +void SSmotor_monitoring(SSstat *inistat); +int SSlog_motor_data(SSstat *old, double *told); + +void SSstart_emulation(double ha_start, double dec_start); diff --git a/Auxiliary_utils/SiderealServoII/main.c b/Auxiliary_utils/SiderealServoII/main.c index bdce1cb..15fe1b8 100644 --- a/Auxiliary_utils/SiderealServoII/main.c +++ b/Auxiliary_utils/SiderealServoII/main.c @@ -21,50 +21,32 @@ #include // exit, free #include // strdup #include // sleep -#include #include "cmdlnopts.h" +#include "emulator.h" +#include "motlog.h" #include "sidservo.h" -/* -#include // tcsetattr -#include // tcsetattr, close, read, write -#include // ioctl -#include // printf, getchar, fopen, perror -#include // exit -#include // read -#include // read -#include // signal -#include // time -#include // memcpy -#include // int types -#include // gettimeofday -*/ - -/** - * This is an example of usage: - * - command line arguments, - * - log file, - * - check of another file version running, - * - signals management, - * - serial port reading/writing. - * The `cmdlnopts.[hc]` are intrinsic files of this demo. - */ - static glob_pars *GP = NULL; // for GP->pidfile need in `signals` /** * We REDEFINE the default WEAK function of signal processing */ void signals(int sig){ + DBG("Quit"); if(sig > 0){ signal(sig, SIG_IGN); DBG("Get signal %d, quit.\n", sig); } LOGERR("Exit with status %d", sig); + DBG("unlink"); if(GP && GP->pidfile) // remove unnesessary PID file unlink(GP->pidfile); + SSwritecmd(CMD_STOPHA); + SSwritecmd(CMD_STOPDEC); + DBG("close"); SSclose(); + DBG("exit"); exit(sig); } @@ -76,22 +58,61 @@ int main(int argc, char *argv[]){ initial_setup(); char *self = strdup(argv[0]); GP = parse_args(argc, argv); -/* if(GP->rest_pars_num){ - printf("%d extra options:\n", GP->rest_pars_num); - for(int i = 0; i < GP->rest_pars_num; ++i) - printf("%s\n", GP->rest_pars[i]); - }*/ check4running(self, GP->pidfile); - red("%s started, snippets library version is %s\n", self, sl_libversion()); + green("%s started, snippets library version is %s\n", self, sl_libversion()); free(self); + if(GP->logfile){ + if(!OPENLOG(GP->logfile, LOGLEVEL_ANY, 1)) ERRX("Can't open logfile %s", GP->logfile); + } + if(GP->motorslog){ + if(!open_mot_log(GP->motorslog)) ERRX("Can't open motors' log file %s", GP->motorslog); + mot_log(0, "# Motor's data\n#time\tX\tXenc\tVx\tY\tYenc\tVy"); + } 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 - if(GP->logfile) OPENLOG(GP->logfile, LOGLEVEL_ANY, 1); LOGMSG("Start application..."); if(!SSinit(GP->device, GP->speed)) signals(-2); + mot_log(0, "Starting of emulation"); + SSstart_emulation(0., 0.); + mot_log(0, "Return to zero"); + SSgoto(0., 0.); + SSwaitmoving(); + signals(0); + // try to move for 45 degrees by both axes + if(!SSgoto(45., 45.)) signals(-3); + SSwaitmoving(); + signals(0); + DBG("Try to send short command"); + SSscmd sc = { + .DECmot = 500000, + .DECspeed = 1000000, + .HAmot = 600000, + .HAspeed = 2000000 + }; + mot_log(0, "Send short command"); + while(SScmds(&sc) != sizeof(SSstat)) WARNX("SSCMDshort bad answer!"); + SSmotor_monitoring((SSstat*)SSread(NULL)); // monitor + SSlcmd lc = { + .DECmot = 0, + .DECspeed = 2000000, // steps per sec * 65536 / 1953 + .HAmot = 0, //-427643 + .HAspeed = 2000000, + .DECadder = 100, + .HAadder = 40, + .DECatime = 1953*3, + .HAatime = 1953*4 + }; + DBG("Try to send long command"); + mot_log(0, "Send long command"); + while(SScmdl(&lc) != sizeof(SSstat)) WARNX("SSCMDlong bad answer!"); + SSmotor_monitoring((SSstat*)SSread(NULL)); // monitor + mot_log(0, "Stop motors"); + SSwritecmd(CMD_STOPHA); + SSwritecmd(CMD_STOPDEC); + SSmotor_monitoring(NULL); // monitor stopping /* double t0 = dtime(); while(1){ // read data from port and print in into terminal diff --git a/Auxiliary_utils/SiderealServoII/mot.log b/Auxiliary_utils/SiderealServoII/mot.log new file mode 100644 index 0000000..1abc2a2 --- /dev/null +++ b/Auxiliary_utils/SiderealServoII/mot.log @@ -0,0 +1,210 @@ +# Motor's data +#time X Xenc Vx Y Yenc Vy +Send short command + 13303.19 498304 54650237 5641.4 660460 47372412 -9664.9 + 13373.17 498816 54650237 7316.6 659711 47361345 -10703.4 + 13443.17 499360 54650237 7770.3 658757 47346256 -13626.6 + 13513.06 499739 54650237 5423.6 657616 47327322 -16328.0 + 13583.08 499955 54650237 3084.7 656236 47303304 -19707.5 + 13655.18 500000 54650237 624.1 654619 47276259 -22427.5 + 13726.26 500000 54650237 0.0 652752 47261344 -26265.0 + 13798.17 500000 54650237 0.0 650714 47227967 -28340.6 + 13868.11 500000 54650237 0.0 648390 47190361 -33227.5 + 13940.23 500000 54650237 0.0 645918 47148884 -34279.1 + 14012.23 500000 54650237 0.0 643167 47103676 -38213.0 + 14082.23 500000 54650237 0.0 640163 47054992 -42910.1 + 14153.09 500000 54650237 0.0 637023 47027434 -44312.0 + 14224.13 500000 54650237 0.0 633635 46971536 -47690.1 + 14295.08 500000 54650237 0.0 630023 46912563 -50906.3 + 14366.06 500000 54650237 0.0 626501 46853184 -49618.2 + 14437.97 500000 54650237 0.0 623163 46796212 -46421.7 + 14507.91 500000 54650237 0.0 620040 46770651 -44652.5 + 14578.13 500000 54650237 0.0 617192 46720784 -40557.1 + 14649.02 500000 54650237 0.0 614571 46675824 -36971.2 + 14719.00 500000 54650237 0.0 612137 46633697 -34782.4 + 14790.15 500000 54650237 0.0 609967 46597060 -30498.5 + 14862.12 500000 54650237 0.0 607957 46563570 -27933.8 + 14932.07 500000 54650237 0.0 606159 46548481 -25701.5 + 15003.06 500000 54650237 0.0 604609 46521327 -21831.6 + 15074.08 500000 54650237 0.0 603289 46497742 -18587.6 + 15144.17 500000 54650237 0.0 602170 46478563 -15965.9 + 15214.98 500000 54650237 0.0 601305 46463679 -12215.6 + 15284.90 500000 54650237 0.0 600642 46457605 -9481.3 + 15355.01 500000 54650237 0.0 600207 46448898 -6205.4 + 15426.00 500000 54650237 0.0 599985 46444326 -3126.9 + 15496.01 500000 54650237 0.0 599990 46444087 71.4 + 15565.85 500000 54650237 0.0 599990 46444035 0.0 + 15635.82 500000 54650237 0.0 599991 46444034 14.3 + 15706.88 500000 54650237 0.0 599991 46444033 0.0 + 15776.82 500000 54650237 0.0 599991 46444022 0.0 + 15846.82 500000 54650237 0.0 599991 46444022 0.0 +Send long command + 16131.76 499414 54650237 -6096.5 599054 46432705 -10048.0 + 16201.78 498878 54650237 -7655.6 598288 46420698 -10940.7 + 16272.79 498181 54650237 -9814.4 597315 46404769 -13700.8 + 16343.78 497308 54650237 -12297.2 596112 46384782 -16945.6 + 16414.63 496267 54650237 -14693.6 594681 46373403 -20198.4 + 16485.66 495078 54650237 -16740.8 593030 46346714 -23245.7 + 16556.65 493700 54650237 -19409.3 591170 46316243 -26198.3 + 16626.71 492150 54650237 -22124.5 589065 46282021 -30046.5 + 16698.68 490492 54650237 -23037.1 586787 46243756 -31651.7 + 16770.52 488593 54650237 -26433.0 584218 46201674 -35759.0 + 16840.52 486529 54650237 -29485.2 581499 46179979 -38842.2 + 16911.46 484354 54650237 -30662.0 578541 46130642 -41700.3 + 16982.52 482046 54650237 -32477.7 575343 46078292 -45001.5 + 17052.52 479510 54650237 -36229.1 571898 46021592 -49215.0 + 17123.55 476842 54650237 -37560.6 568330 45961068 -50231.0 + 17193.56 473952 54650237 -41281.0 564449 45896595 -55436.5 + 17263.58 470973 54650237 -42543.1 560409 45862243 -57695.3 + 17335.53 467810 54650237 -43966.6 556266 45791085 -57588.9 + 17405.49 464397 54650237 -48783.6 551976 45720984 -61319.0 + 17475.53 460914 54650237 -49729.4 547776 45649988 -59966.6 + 17545.57 457269 54650237 -52037.3 543638 45579416 -59075.5 + 17616.50 453463 54650237 -53660.0 539446 45543487 -59102.1 + 17687.50 449418 54650237 -56971.8 535222 45472384 -59492.9 + 17759.55 445244 54650237 -57936.9 530968 45402024 -59047.3 + 17831.41 440932 54650237 -60003.9 526697 45330704 -59433.4 + 17902.45 436674 54650237 -59937.1 522394 45260321 -60570.6 + 17974.42 432430 54650237 -58969.0 518180 45189210 -58552.2 + 18045.43 428105 54650237 -60905.2 513883 45153563 -60510.9 + 18117.42 423873 54650237 -58785.2 509641 45083156 -58924.1 + 18189.41 419603 54650237 -59310.6 505378 45011833 -59213.4 + 18260.41 415305 54650237 -60541.0 501074 44941371 -60625.5 + 18332.29 411098 54650237 -58523.4 496843 44870325 -58857.2 + 18404.29 406801 54650237 -59683.7 492542 44799294 -59739.3 + 18474.42 402518 54650237 -61069.7 488237 44764352 -61383.4 + 18546.29 398321 54650237 -58396.5 484095 44693048 -57631.2 + 18617.26 394020 54650237 -60608.9 479796 44622609 -60580.7 + 18688.31 389822 54650237 -59079.3 475597 44551752 -59093.3 + 18759.37 385581 54650237 -59682.8 471356 44480519 -59682.8 + 18831.32 381332 54650237 -59056.7 467106 44410023 -59070.6 + 18903.32 377052 54650237 -59442.7 462825 44374362 -59456.5 + 18974.35 372760 54650237 -60426.1 458528 44303795 -60496.5 + 19046.31 368506 54650237 -59117.0 454282 44232986 -59005.8 + 19118.29 364248 54650237 -59157.9 449997 44161769 -59533.0 + 19189.31 359934 54650237 -60743.3 445703 44091235 -60461.7 + 19261.19 355712 54650237 -58736.9 441458 44020045 -59056.9 + 19332.18 351422 54650237 -60427.5 437193 43984937 -60075.4 + 19404.29 347175 54650237 -58901.8 432950 43914065 -58846.4 + 19475.19 342875 54650237 -60645.6 428652 43842798 -60617.4 + 19547.36 338680 54650237 -58123.4 424427 43772518 -58539.0 + 19618.22 334365 54650237 -60900.7 420129 43701253 -60660.8 + 19688.31 330148 54650237 -60158.5 415897 43630749 -60372.5 + 19759.28 325990 54650237 -58589.9 411734 43595366 -58660.3 + 19830.34 321736 54650237 -59871.6 407505 43523993 -59519.7 + 19902.21 317482 54650237 -59187.8 403260 43453664 -59062.6 + 19974.27 313226 54650237 -59058.7 398970 43382500 -59530.5 + 20045.35 308930 54650237 -60446.7 394670 43312027 -60503.0 + 20116.33 304671 54650237 -60000.1 390438 43241065 -59619.8 + 20188.32 300465 54650237 -58421.5 386228 43205317 -58477.0 + 20259.33 296162 54650237 -60597.2 381910 43135048 -60808.4 + 20329.35 291909 54650237 -60736.3 377685 43063716 -60336.4 + 20400.39 287774 54650237 -58211.6 373519 42993191 -58648.0 + 20471.33 283528 54650237 -59847.5 369300 42922673 -59467.0 + 20541.34 279284 54650237 -60623.3 365056 42851557 -60623.3 + 20612.36 275098 54650237 -58942.8 360869 42816352 -58956.9 + 20684.33 270883 54650237 -58569.4 356652 42745135 -58597.2 + 20756.25 266580 33057240 -59827.1 352349 42666083 -59827.1 + 20828.38 262327 33057240 -58957.9 348072 42595149 -59290.6 + 20898.22 258007 33057240 -61859.3 343778 42559373 -61487.0 + 20970.35 253831 33057240 -57896.3 339602 42488934 -57896.3 + 21041.36 249523 33057240 -60668.4 335323 42417665 -60260.0 + 21111.23 245330 33057240 -60013.9 331075 42347376 -60801.1 + 21181.16 241128 33057240 -60080.2 326900 42276464 -59694.1 + 21252.23 236994 33057240 -58167.8 322713 42205173 -58913.6 + 21323.38 232744 33057240 -59734.7 318515 42170159 -59003.8 + 21393.36 228489 33057240 -60808.3 314272 42099029 -60636.9 + 21465.39 224314 33057240 -57961.9 310085 42028722 -58128.5 + 21536.38 220049 33057240 -60078.0 305823 41958020 -60035.7 + 21606.16 215804 33057240 -60833.3 301577 41886742 -60847.7 + 21677.40 211651 33057240 -58303.2 297398 41851757 -58668.2 + 21748.36 207418 33057240 -59644.9 293193 41780888 -59250.3 + 21820.37 203177 33057240 -58893.0 288926 41710378 -59254.1 + 21892.39 198885 33057240 -59592.9 284623 41639413 -59745.7 + 21962.35 194595 33057240 -61321.7 280359 41568240 -60950.1 + 22034.40 190417 33057240 -57988.2 276196 41498263 -57780.0 + 22106.38 186118 33057240 -59723.3 271865 41426639 -60167.9 + 22177.37 181833 33057240 -60368.2 267603 41391722 -60044.2 + 22249.38 177612 33057240 -58614.3 263362 41320779 -58892.0 + 22320.40 173309 33057240 -60587.8 259057 41249716 -60616.0 + 22391.14 169100 33057240 -59500.4 254840 41179597 -59613.5 + 22463.23 164870 33057240 -58675.8 250609 41108006 -58689.6 + 22532.89 160578 33057240 -61613.6 246325 41037724 -61498.8 + 22602.89 156422 33057240 -59373.1 242145 41002089 -59716.0 + 22674.88 152226 33057240 -58289.9 238000 40931214 -57581.4 + 22744.64 147930 33057240 -61572.0 233699 40860866 -61643.6 + 22815.62 143783 33057240 -58434.9 229552 40789182 -58434.9 + 22886.62 139535 33057240 -59827.7 225306 40719211 -59799.5 + 22958.62 135328 33057240 -58426.5 221097 40683165 -58454.2 + 23030.64 131033 33057240 -59642.9 216801 40612638 -59656.8 + 23102.47 126725 33057240 -59973.1 212499 40542177 -59889.6 + 23174.52 122466 33057240 -59112.6 208210 40470560 -59529.0 + 23244.52 118136 33057240 -61855.3 203915 40400739 -61355.3 + 23314.52 114010 33057240 -58936.1 199754 40329899 -59436.0 + 23386.56 109816 33057240 -58223.4 195589 40294238 -57820.8 + 23456.55 105521 33057240 -61363.3 191289 40223595 -61434.7 + 23527.53 101356 33057240 -58675.3 187126 40152087 -58647.1 + 23597.51 97130 33057240 -60394.7 182963 40082112 -59494.4 + 23668.53 92936 33057240 -59053.7 178758 40011123 -59208.6 + 23739.52 88709 33057240 -59551.9 174475 39939518 -60340.9 + 23809.39 84489 33057240 -60385.9 170241 39904730 -60586.3 + 23879.52 80315 33057240 -59516.3 166145 39833693 -58404.1 + 23950.37 76118 33057240 -59246.1 161890 39763522 -60064.8 + 24021.56 71921 33057240 -58955.0 157723 39692341 -58533.6 + 24092.41 67677 33057240 -59902.9 153457 39621040 -60213.4 + 24162.36 63423 33057240 -60809.6 149195 39585914 -60924.0 + 24233.40 59287 33057240 -58219.8 145108 39515260 -57530.1 + 24304.39 55037 33057240 -59862.7 140807 39444968 -60581.0 + 24375.41 50866 33057240 -58729.8 136629 39373730 -58828.4 + 24445.27 46821 33057240 -57901.5 132337 39302707 -61437.2 + 24516.41 42989 33057240 -53867.2 128237 39267360 -57634.5 + 24586.29 39273 33057240 -53180.7 123961 39196956 -61195.0 + 24656.37 35810 33057240 -49414.3 119821 39126495 -59074.5 + 24727.44 32464 33057240 -47080.4 115652 39054998 -58660.5 + 24798.30 29237 33057240 -45540.4 111353 38984040 -60668.8 + 24868.27 26217 33057240 -43162.0 107167 38914545 -59826.5 + 24937.30 23370 33057240 -41239.9 103023 38878374 -60027.5 + 25006.29 20697 33057240 -38744.2 98899 38807968 -59776.0 + 25075.42 18231 33057240 -35673.9 94773 38736609 -59688.0 + 25146.43 15951 33057240 -32110.9 90641 38665667 -58194.0 + 25215.39 13709 33057240 -32508.8 86455 38630251 -60696.7 + 25284.41 11683 33057240 -29352.1 82338 38559921 -59645.8 + 25354.33 9862 33057240 -26046.7 78215 38489376 -58973.4 + 25424.41 8139 33057240 -24587.6 74059 38417898 -59307.0 + 25493.44 6577 33057240 -22625.5 69870 38347238 -60677.4 + 25563.43 5198 33057240 -19701.7 65752 38311350 -58833.6 + 25632.47 3968 33057240 -17817.1 61579 38241650 -60447.8 + 25702.44 2920 33057240 -14977.8 57475 38170479 -58653.6 + 25772.42 2019 33057240 -12873.5 53306 38099165 -59566.5 + 25841.44 1282 33057240 -10679.3 49139 38028924 -60380.9 + 25908.49 715 33057240 -8457.5 45069 37992837 -60709.0 + 25978.43 317 33057240 -5689.7 41014 37922920 -57969.3 + 26046.42 52 33057240 -3897.5 37005 37852774 -58961.8 + 26114.46 11 33057240 -602.6 33219 37786211 -55645.4 + 26181.42 10 33057240 -14.9 29638 37755569 -53477.3 + 26250.45 8 33057240 -29.0 26314 37694617 -48160.6 + 26317.46 0 33057240 -119.4 23132 37638936 -47477.7 + 26383.40 0 33057240 0.0 20198 37612621 -44500.4 + 26450.46 0 33057240 0.0 17539 37562913 -39647.5 + 26517.49 0 33057240 0.0 14998 37517006 -37910.6 + 26585.46 0 33057240 0.0 12644 37475088 -34633.5 + 26651.45 0 33057240 0.0 10454 37455790 -33188.3 + 26718.31 0 33057240 0.0 8532 37420115 -28744.9 + 26784.44 0 33057240 0.0 6783 37388019 -26446.3 + 26853.42 0 33057240 0.0 5241 37360557 -22355.6 + 26921.30 0 33057240 0.0 3878 37348346 -20079.9 + 26987.34 0 33057240 0.0 2728 37327218 -17414.2 + 27054.36 0 33057240 0.0 1798 37310184 -13875.0 + 27121.32 0 33057240 0.0 1043 37303173 -11275.1 + 27187.43 0 33057240 0.0 492 37292118 -8334.6 + 27252.47 0 33057240 0.0 145 37285086 -5335.5 + 27319.49 0 33057240 0.0 9 37282145 -2029.3 + 27387.32 0 33057240 0.0 10 37282191 14.7 + 27454.45 0 33057240 0.0 8 37282196 -29.8 + 27520.46 0 33057240 0.0 0 37282182 -121.2 + 27586.48 0 33057240 0.0 0 37282177 0.0 + 27652.48 0 33057240 0.0 0 37282168 0.0 + 27718.50 0 33057240 0.0 0 37282170 0.0 +Stop motors + 27960.49 0 33057240 0.0 0 37282175 0.0 + 28026.49 0 33057240 0.0 0 37282174 0.0 diff --git a/Auxiliary_utils/SiderealServoII/motlog.c b/Auxiliary_utils/SiderealServoII/motlog.c new file mode 100644 index 0000000..ca81336 --- /dev/null +++ b/Auxiliary_utils/SiderealServoII/motlog.c @@ -0,0 +1,69 @@ +/* + * This file is part of the SSII project. + * Copyright 2022 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 "motlog.h" + +static FILE *flog = NULL; +static double time0 = 0.; + +int open_mot_log(const char *path){ + if(!path) return FALSE; + flog = fopen(path, "a+"); + if(!flog){ + WARN("Can't open %s", path); + return FALSE; + } + time0 = dtime(); + return TRUE; +} + +void refresh_t0(){ + time0 = dtime(); +} + +void close_mot_log(){ + if(!flog) return; + fclose(flog); + flog = NULL; +} + +int mot_log(int timestamp, const char *fmt, ...){ + if(!flog) return 0; + int i = 0; + // timestamp in milliseconds +#ifdef EBUG + red("Time: %10.2f\n", 1000.*(dtime() - time0)); +#endif + if(timestamp) i += fprintf(flog, "%10.2f\t", 1000.*(dtime() - time0)); + va_list ar; + va_start(ar, fmt); + i += vfprintf(flog, fmt, ar); + va_end(ar); + fseek(flog, -1, SEEK_CUR); + char c; + ssize_t r = fread(&c, 1, 1, flog); + if(1 == r){ // add '\n' if there was no newline + if(c != '\n') i += fprintf(flog, "\n"); + } + fflush(flog); + return i; +} diff --git a/Auxiliary_utils/SiderealServoII/motlog.h b/Auxiliary_utils/SiderealServoII/motlog.h new file mode 100644 index 0000000..ccf0645 --- /dev/null +++ b/Auxiliary_utils/SiderealServoII/motlog.h @@ -0,0 +1,25 @@ +/* + * This file is part of the SSII project. + * Copyright 2022 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 + +int open_mot_log(const char *path); +void close_mot_log(); +int mot_log(int timestamp, const char *fmt, ...); +void refresh_t0(); diff --git a/Auxiliary_utils/SiderealServoII/msg.log b/Auxiliary_utils/SiderealServoII/msg.log new file mode 100644 index 0000000..593e3a8 --- /dev/null +++ b/Auxiliary_utils/SiderealServoII/msg.log @@ -0,0 +1,3 @@ +[MSG] 2022/07/30-21:16:04 Start application... +[DBG] 2022/07/30-21:16:04 Try to open serial /dev/ttyUSB0 @19200 +[ERR] 2022/07/30-21:16:32 Exit with status 0 diff --git a/Auxiliary_utils/SiderealServoII/sidservo.c b/Auxiliary_utils/SiderealServoII/sidservo.c index c450720..25283db 100644 --- a/Auxiliary_utils/SiderealServoII/sidservo.c +++ b/Auxiliary_utils/SiderealServoII/sidservo.c @@ -16,57 +16,80 @@ * along with this program. If not, see . */ +#include #include #include +#include +#include "emulator.h" +#include "motlog.h" #include "sidservo.h" static TTY_descr *dev = NULL; // shoul be global to restore if die static uint8_t buff[BUFLEN]; static int buflen = 0; -static uint16_t calcChecksum(uint8_t *buf, int len){ - uint16_t checksum = 0x11; // I don't know from where does this "magick" +static uint16_t calcChecksum(uint8_t initval, uint8_t *buf, int len){ + uint16_t checksum = initval; // I don't know from where does this "magick" for(int i = 0; i < len; i++) checksum += *buf++; checksum ^= 0xFF00; // invert high byte - DBG("Checksum: 0x%04x", checksum); + DBG("Checksum of %d bytes: 0x%04x", len, checksum); return checksum; } +// encoder's settings per rev +static struct { + uint32_t hamotperrev; // motors' encoders ticks per revolution + uint32_t decmotperrev; + uint32_t haencperrev; // axis' encoders ticks per revolution + uint32_t decencperrev; + int32_t hamotzero; // motors' encoders ticks @ zero position + int32_t decmotzero; +} encsettings; + /** * @brief SSinit - open serial device and get initial info * @return TRUE if all OK */ int SSinit(char *devpath, int speed){ LOGDBG("Try to open serial %s @%d", devpath, speed); + int64_t i; dev = new_tty(devpath, speed, BUFLEN); if(dev) dev = tty_open(dev, 1); // open exclusively if(!dev){ LOGERR("Can't open %s with speed %d. Exit.", devpath, speed); signals(-1); } - for(int ntries = 0; ntries < 5; ++ntries){ // try at most 5 times - if(!SSgetstat(NULL)) continue; - SSstat *s = (SSstat*) buff; -#ifdef EBUG - green("\nGet data:\n"); - printf("DECmot=%d (0x%08x)\n", s->DECmot, (uint32_t)s->DECmot); - printf("DECenc=%d (0x%08x)\n", s->DECenc, (uint32_t)s->DECenc); - printf("RAmot=%d (0x%08x)\n", s->RAmot, (uint32_t)s->RAmot); - printf("RAenc=%d (0x%08x)\n", s->RAenc, (uint32_t)s->RAenc); - printf("keypad=0x%02x\n", s->keypad); - printf("XBits=0x%02x\n", s->XBits); - printf("YBits=0x%02x\n", s->YBits); - printf("EBits=0x%02x\n", s->ExtraBits); - printf("ain0=%u\n", s->ain0); - printf("ain1=%u\n", s->ain1); - printf("millis=%u\n", s->millis); - printf("tF=%d\n", s->tF); - printf("V=%f\n", ((float)s->voltage)/10.f); - printf("checksum=0x%04x\n\n", s->checksum); -#endif - if(calcChecksum(buff, sizeof(SSstat)-2) == s->checksum) return TRUE; + for(int ntries = 0; ntries < 10; ++ntries){ // try at most 10 times + SSstat s; + DBG("Try for %dth time... ", ntries); + SSwritecmd(CMD_MOTHA); + SSwritecmd(CMD_MOTDEC); + if(!SSgetPartialstat(&s)) continue; + if(!SSgetInt(CMD_GETDECMEPR, &i)) continue; + encsettings.decmotperrev = (uint32_t) i; + LOGDBG("decmotperrev = %u", encsettings.decmotperrev); + if(!SSgetInt(CMD_GETHAMEPR, &i)) continue; + encsettings.hamotperrev = (uint32_t) i; + LOGDBG("hamotperrev = %u", encsettings.hamotperrev); + if(!SSgetInt(CMD_GETDECAEPR, &i)) continue; + encsettings.decencperrev = (uint32_t) i; + LOGDBG("decencperrev = %u", encsettings.decencperrev); + if(!SSgetInt(CMD_GETHAAEPR, &i)) continue; + encsettings.haencperrev = (uint32_t) i; + LOGDBG("haencperrev = %u", encsettings.haencperrev); + double d = (double)(s.HAenc - HA_ENC_ZEROPOS); + d /= encsettings.haencperrev; + d *= encsettings.hamotperrev; + encsettings.hamotzero = s.HAmot - (int32_t)d; + LOGDBG("hamotzero = %d", encsettings.hamotzero); + d = (double)(s.DECenc - DEC_ENC_ZEROPOS); + d /= encsettings.decencperrev; + d *= encsettings.decmotperrev; + encsettings.decmotzero = s.DECmot - (int32_t)d; + LOGDBG("decmotzero = %u", encsettings.decmotzero); + return TRUE; } return FALSE; } @@ -79,18 +102,32 @@ void SSclose(){ } /** - * @brief SSwrite - write & return answer + * @brief SSwrite - write command and wait for answer * @param buf - buffer with text or binary data - * @param buflen - its length + * @param len - its length + * @param flags - >0: wait for binary data (until timeout), ==0: wait for '\r' or timeout, <0: don't wait at all * @return amount of bytes read, if got answer; 0 without answer, -1 if device disconnected, -2 if can't write */ -int SSwrite(const uint8_t *buf, int len){ +int SSwrite(const uint8_t *buf, int len, int flags){ DBG("try to write %d bytes", len); - if(write_tty(dev->comfd, (const char*)buf, len)){ +#ifdef EBUG + double t00 = dtime(); + for(int i = 0; i < len; ++i) if(buf[i] > 32) printf("%c", buf[i]); else printf("."); + printf("\n"); + for(int i = 0; i < len; ++i) printf("0x%02x ", buf[i]); + printf("\n"); +#endif + read_tty(dev); // clear incoming buffer + if(write_tty(dev->comfd, (const char*)buf, (size_t) len)){ LOGERR("Can't write data to port"); + DBG("Can't write, time=%g", dtime()-t00); return -2; } write_tty(dev->comfd, "\r", 1); // add EOL + if(flags < 0){ + DBG("Don't wait, time=%g", dtime()-t00); + return 0; + } buflen = 0; double t0 = dtime(); while(dtime() - t0 < READTIMEOUT && buflen < BUFLEN){ @@ -101,11 +138,20 @@ int SSwrite(const uint8_t *buf, int len){ } if(r == 0) continue; int rest = BUFLEN - buflen; - if((int)dev->buflen > rest) dev->buflen = rest; // TODO: what to do with possible buffer overrun? + int out = 0; + DBG("buf[%d]=0x%02x", r-1, dev->buf[r-1]); + if(flags == 0 && dev->buf[r-1] == '\n') out = 1; // end of message + if((int)dev->buflen > rest){ + dev->buflen = (size_t) rest; + out = 1; + } memcpy(&buff[buflen], dev->buf, dev->buflen); buflen += dev->buflen; + DBG("get %ld bytes, out=%d", dev->buflen, out); + if(out) break; + t0 = dtime(); } - DBG("got buflen=%d", buflen); + DBG("got buflen=%d, time=%g", buflen, dtime()-t00); #ifdef EBUG for(int i = 0; i < buflen; ++i){ printf("%02x (%c) ", buff[i], (buff[i] > 31) ? buff[i] : ' '); @@ -115,24 +161,288 @@ int SSwrite(const uint8_t *buf, int len){ return buflen; } +// write simple command with string answer +int SSwritecmd(const uint8_t *cmd){ + return SSwrite(cmd, (int)strlen((const char*)cmd), 0); +} + +// write command and get answer +int SSgetInt(const uint8_t *cmd, int64_t *ans){ + int r = SSwritecmd(cmd); + if(r > 0 && ans){ + int l = 0; + char *ptr = (char*) SSread(&l); + ptr[l] = 0; + while(l){ + if(isdigit(*ptr)) break; + ++ptr; ++l; + } + if(l == 0) return 0; // no integer found + *ans = atol(ptr); + DBG("from %s get answer: %ld", buff, *ans); + } + return r; +} + /** * @brief SSread - return buff and buflen * @param l (o) - length of data * @return buff or NULL if buflen == 0 */ uint8_t *SSread(int *l){ - if(l) *l = buflen; if(!buflen) return NULL; + if(l) *l = buflen; return buff; } +// fill some fields of `s` by stupid string commands +int SSgetPartialstat(SSstat *s){ + SSstat st = {0}; + int64_t par; + int r = 0; +#define GET(cmd, field) do{if(SSgetInt(cmd, &par)){st.field = (int32_t)par; ++r;}}while(0) + GET(CMD_MOTDEC, DECmot); + GET(CMD_MOTHA, HAmot); + GET(CMD_ENCDEC, DECenc); + GET(CMD_ENCHA, HAenc); +#undef GET + if(s) *s = st; +#ifdef EBUG + if(s){ + green("\nGet data:\n"); + printf("DECmot=%d (0x%08x)\n", s->DECmot, (uint32_t)s->DECmot); + printf("DECenc=%d (0x%08x)\n", s->DECenc, (uint32_t)s->DECenc); + printf("RAmot=%d (0x%08x)\n", s->HAmot, (uint32_t)s->HAmot); + printf("RAenc=%d (0x%08x)\n", s->HAenc, (uint32_t)s->HAenc); + } +#endif + return r; // amount of fields with good data +} + +static void parsestat(SSstat *s){ + green("\nGet data:\n"); + printf("DECmot=%d (0x%08x)\n", s->DECmot, (uint32_t)s->DECmot); + printf("DECenc=%d (0x%08x)\n", s->DECenc, (uint32_t)s->DECenc); + printf("DECLast=%d (0x%08x)\n", s->DecLast, (uint32_t)s->DecLast); + printf("RAmot=%d (0x%08x)\n", s->HAmot, (uint32_t)s->HAmot); + printf("RAenc=%d (0x%08x)\n", s->HAenc, (uint32_t)s->HAenc); + printf("RALast=%d (0x%08x)\n", s->HALast, (uint32_t)s->HALast); + printf("keypad=0x%02x\n", s->keypad); + printf("XBits=0x%02x\n", s->XBits); + printf("YBits=0x%02x\n", s->YBits); + printf("EBits=0x%02x\n", s->ExtraBits); + printf("ain0=%u\n", s->ain0); + printf("ain1=%u\n", s->ain1); + printf("millis=%u\n", s->millis); + printf("tF=%d\n", s->tF); + printf("V=%f\n", ((float)s->voltage)/10.f); + printf("checksum=0x%04x\n\n", s->checksum); +} + /** * @brief SSgetstat - get struct with status & check its crc * @param s - pointer to allocated struct (or NULL just to check) * @return 1 if OK */ int SSgetstat(SSstat *s){ - if(SSwrite(CMD_GETSTAT, sizeof(CMD_GETSTAT)) != sizeof(SSstat)) return FALSE; - if(s) memcpy(s, buff, sizeof(SSstat)); + int r = SSwrite(CMD_GETSTAT, 3, 1); + SSstat *n; + switch(r){ + case sizeof(SSstat): + n = (SSstat*) buff; + if(calcChecksum(0, buff, sizeof(SSstat)-2) != n->checksum) return FALSE; + if(s) memcpy(s, buff, sizeof(SSstat)); + break; + default: return FALSE; // wrong answer size + } +#ifdef EBUG + if(s){ + parsestat(s); + } +#endif return TRUE; } + +// send short/long binary command +static int bincmd(void *cmd, int len){ + //uint8_t *buf = MALLOC(uint8_t, len + 3); + if(len == sizeof(SSscmd)){ + ((SSscmd*)cmd)->checksum = calcChecksum(0, cmd, len-2); + SSwrite(CMD_SHORTCMD, 3, -1); + //memcpy(buf, CMD_SHORTCMD, 3); + //memcpy(buf+3, cmd, len); + }else if(len == sizeof(SSlcmd)){ + ((SSlcmd*)cmd)->checksum = calcChecksum(0, cmd, len-2); + SSwrite(CMD_LONGCMD, 3, -1); + //memcpy(buf, CMD_LONGCMD, 3); + //memcpy(buf+3, cmd, len); + }else{ + //free(buf); + return -1; + } + //len += 3; + DBG("Write %d bytes", len); + int r = SSwrite(cmd, len, 1); +#ifdef EBUG + if(r == sizeof(SSscmd)) parsestat((SSstat*)buff); +#endif + return r; +} +int SScmds(SSscmd *cmd){ + int r = bincmd(cmd, sizeof(SSscmd)); + if(r < 0) return -1; + DBG("Got %d bytes answer", r); + return r; +} +int SScmdl(SSlcmd *cmd){ + int r = bincmd(cmd, sizeof(SSlcmd)); + if(r < 0) return -1; + DBG("Got %d bytes answer", r); + return r; +} + +// return d in 0..360 for isdec==1 and -180..180 for isdec==0 +static double normangle(double d, int isdec){ + if(d < -360.) d = fmod((fmod(d, 360.) + 360.), 360.); + else d = fmod(d + 360., 360.); + if(isdec) return d; + if(d < 180.) return d; + return (d - 180.); +} + +/** + * @brief SSticks2deg - convert motor ticks to degrees + * @param ticks - motor ticks + * @param isdec == 1 if it is DEC axe + * @return degrees + */ +double SSticks2deg(int32_t ticks, int isdec){ + double denom = (isdec) ? encsettings.decmotperrev : encsettings.hamotperrev; + int32_t zp = (isdec) ? encsettings.decmotzero : encsettings.hamotzero; + return normangle(360. * (ticks - zp) / denom, isdec); +} +/** + * @brief SSenc2deg - convert encoder ticks to degrees + * @param ticks - motor ticks + * @param isdec == 1 if it is DEC axe + * @return degrees + */ +double SSenc2deg(int32_t ticks, int isdec){ + double denom = (isdec) ? encsettings.decencperrev : encsettings.haencperrev; + int32_t zp = (isdec) ? DEC_ENC_ZEROPOS : HA_ENC_ZEROPOS; + return normangle(360. * (ticks - zp) / denom, isdec); +} + +/** + * @brief SSdeg2ticks - convert degrees to motor ticks + * @param d - angle + * @param isdec == 1 if it is DEC axe + * @return ticks + */ +int32_t SSdeg2ticks(double d, int isdec){ + double k = (isdec) ? encsettings.decmotperrev : encsettings.hamotperrev; + int32_t zp = (isdec) ? encsettings.decmotzero : encsettings.hamotzero; + return (int32_t)(k * normangle(d, isdec) / 360.) + zp; + +} +/** + * @brief SSdeg2enc - convert degrees to encoder ticks + * @param d - angle + * @param isdec == 1 if it is DEC axe + * @return ticks + */ +int32_t SSdeg2enc(double d, int isdec){ + double k = (isdec) ? encsettings.decencperrev : encsettings.haencperrev; + int32_t zp = (isdec) ? DEC_ENC_ZEROPOS : HA_ENC_ZEROPOS; + return (int32_t)(k * normangle(d, isdec) / 360.) + zp; +} + +// convert speed `dps` in degrees per second into ticks for `XS`/`YS` commands +int32_t SSdeg2spd(double dps, int isdec){ + double k = (isdec) ? encsettings.decmotperrev : encsettings.hamotperrev; + return (int32_t)(k * dps * 65536./1953./360.); +} + +// convert ticks for `XS`/`YS` into degrees per second +double SSspd2deg(int32_t ticks, int isdec){ + ; +} + +/** + * @brief SSgoto - move telescope to given angle + * @param ha - Hour Angle (degrees) + * @param dec - Declination (degrees) + * @return TRUE if OK + */ +int SSgoto(double ha, double dec){ + char buf[32]; + if(ha < 0. || ha > 360.) return FALSE; + if(dec < -90. || dec > 90.) return FALSE; + int32_t raticks = SSdeg2ticks(ha, 0); + int32_t decticks = SSdeg2ticks(dec, 1); + snprintf(buf, 31, "%s%u", CMD_MOTDEC, decticks); + SSwritecmd((uint8_t*)buf); + snprintf(buf, 31, "%s%u", CMD_MOTHA, raticks); + SSwritecmd((uint8_t*)buf); + return TRUE; +} + +// wait till moving ends +void SSwaitmoving(){ + SSstat s = {0}; + double t0 = dtime(); + int32_t oldRAm = 0, oldDm = 0; + int first = 1, ctr = 0; + do{ + if(!SSlog_motor_data(&s, &t0)) continue; + DBG("Moving: HA=%g, DEC=%g", SSenc2deg(s.HAenc, 0), SSenc2deg(s.DECenc, 1)); + if(first) first = 0; + else if(s.HAmot == oldRAm && s.DECmot == oldDm){ + if(++ctr > 2) return; + }else ctr = 0; + oldRAm = s.HAmot; + oldDm = s.DECmot; + }while(dtime() - t0 < 3.); // simplest timeout + DBG("Moving ends (or timeout ends)"); +} + +int SScatchtarg(double ra, double dec){ + ; +} + +#if 0 +The controller loops 1953 times per second. Fields that reference a speed/velocity/rate (e.g. the XS command) express this speed as a 32-bit number representing the number of ticks the motor encoder should advance per loop, multiplied by 2^16 (= 65,536) to express fractional values. + +For example, if you want to advance the motor 1,000 counts per second, this corresponds to: + +1,000 / 1953 ~= 0.512 counts per loop + +which is expressed as the integer value: + + round(0.512 * 65,536) = 33,555 + +So you would send the command ?XS33555\r? to set a max speed of 1,000 counts per second. + +Note that 65,536 / 1,953 = 33.55657962109575, and 1,953 / 65,536 = 0.0298004150390625, so you can simply do: + + CountsPerSecToSpeedValue(cps): return round(cps * 33.55657962109575) + + SpeedValueToCountsPerSec(speed): return round(speed * 0.0298004150390625) + +The SiTech Operations Manual describes the following ?C++? functions (converted to something like Python here) to do similar conversions: + + def DegsPerSec2MotorSpeed(dps, ticksPerRev): + +return round(ticksPerRev * dps * 0.09321272116971) + +def MotorSpeed2DegsPerSec(speed, ticksPerRev): + + return round(speed / ticksPerRev * 10.7281494140625) + +These coefficients can be derived as follows: + + 10.7281494140625 = 0.0298004150390625 * 360 degs/rev + + 0.09321272116971 = 33.55657962109575 / 360 degs/rev + +#endif diff --git a/Auxiliary_utils/SiderealServoII/sidservo.h b/Auxiliary_utils/SiderealServoII/sidservo.h index 8553839..a0f5176 100644 --- a/Auxiliary_utils/SiderealServoII/sidservo.h +++ b/Auxiliary_utils/SiderealServoII/sidservo.h @@ -20,42 +20,106 @@ #ifndef SIDSERVO_H__ #define SIDSERVO_H__ -#include +#include // ASCII commands #define U8P(x) ((uint8_t*)x) // get binary data of all statistics -#define CMD_GETSTAT U8P("XXS") - +#define CMD_GETSTAT U8P("XXS") +// send short command +#define CMD_SHORTCMD U8P("XXR") +// send long command +#define CMD_LONGCMD U8P("YXR") +// get/set HA/DEC in motsteps +#define CMD_MOTDEC U8P("X") +#define CMD_MOTHA U8P("Y") +// -//- in encoders' ticks +#define CMD_ENCDEC U8P("XZ") +#define CMD_ENCHA U8P("YZ") +// normal stop dec/ra +#define CMD_STOPDEC U8P("XN") +#define CMD_STOPHA U8P("YN") +// emergency stop +#define CMD_EMSTOPDEC U8P("XG") +#define CMD_EMSTOPHA U8P("YG") +// getters of motor's encoders per rev +#define CMD_GETDECMEPR U8P("XXU") +#define CMD_GETHAMEPR U8P("XXV") +// -//- axis encoders +#define CMD_GETDECAEPR U8P("XXT") +#define CMD_GETHAAEPR U8P("XXZ") #define BUFLEN (256) -// timeout (seconds) of reading answer -#define READTIMEOUT (0.1) +// timeout (seconds) of reading answer (from last symbol read) +#define READTIMEOUT (0.05) + +// Zero positions of RA/DEC encoderc +#define HA_ENC_ZEROPOS (43066232) +#define DEC_ENC_ZEROPOS (37282120) + + + // all need data in one -typedef struct{ - int32_t DECmot; // Dec/RA motor position - int32_t RAmot; - int32_t DECenc; // Dec/RA encoder position - int32_t RAenc; - uint8_t keypad; // keypad status - uint8_t XBits; - uint8_t YBits; - uint8_t ExtraBits; - uint16_t ain0; // analog inputs - uint16_t ain1; - uint32_t millis; // milliseconds clock - int8_t tF; // temperature (degF) - uint8_t voltage; // input voltage *10 - uint32_t reserved0; - uint32_t reserved1; - uint16_t checksum; // checksum, H inverted +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 + uint8_t keypad; // 17 keypad status + uint8_t XBits; // 18 + uint8_t YBits; // 19 + uint8_t ExtraBits; // 20 + uint16_t ain0; // 21 analog inputs + uint16_t ain1; // 23 + 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 + 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 + uint8_t xychange; // 16 change Xbits/Ybits value + uint8_t XBits; // 17 + uint8_t YBits; // 18 + uint16_t checksum; // 19 +} __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 + uint16_t checksum; // 32 +} __attribute__((packed)) SSlcmd; // long command + int SSinit(char *devpath, int speed); void SSclose(); -int SSwrite(const uint8_t *buf, int len); +int SSwrite(const uint8_t *buf, int len, int isbin); +int SSwritecmd(const uint8_t *cmd); +int SSgetInt(const uint8_t *cmd, int64_t *ans); +int SScmds(SSscmd *cmd); +int SScmdl(SSlcmd *cmd); uint8_t *SSread(int *l); int SSgetstat(SSstat *s); +int SSgetPartialstat(SSstat *s); +double SSticks2deg(int32_t ticks, int isdec); +double SSenc2deg(int32_t ticks, int isdec); +int32_t SSdeg2ticks(double d, int isdec); +int32_t SSdeg2enc(double d, int isdec); +int SSgoto(double ha, double dec); +void SSwaitmoving(); +int SScatchtarg(double ra, double dec); #endif // SIDSERVO_H__