From e898642af9d96fdeadf76b256cf23f9670640812 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Thu, 27 Feb 2025 22:48:27 +0300 Subject: [PATCH] start configurator --- .../LibSidServo/examples/CMakeLists.txt | 11 +- .../LibSidServo/examples/Readme.md | 28 ++ .../LibSidServo/examples/SSIIconf.c | 76 +++++ Auxiliary_utils/LibSidServo/examples/conf.c | 65 +++++ Auxiliary_utils/LibSidServo/examples/conf.h | 27 ++ Auxiliary_utils/LibSidServo/examples/dump.c | 19 +- .../LibSidServo/examples/dumpmoving.c | 34 +-- .../LibSidServo/examples/dumpmoving_scmd.c | 54 ++-- .../LibSidServo/examples/dumpswing.c | 28 +- Auxiliary_utils/LibSidServo/examples/goto.c | 49 +++- .../LibSidServo/examples/scmd_traectory.c | 21 +- .../LibSidServo/libsidservo.creator.user | 2 +- Auxiliary_utils/LibSidServo/libsidservo.files | 3 + Auxiliary_utils/LibSidServo/main.c | 122 +++++++- Auxiliary_utils/LibSidServo/serial.c | 74 ++++- Auxiliary_utils/LibSidServo/serial.h | 2 + Auxiliary_utils/LibSidServo/sidservo.h | 90 +++++- Auxiliary_utils/LibSidServo/ssii.c | 45 +-- Auxiliary_utils/LibSidServo/ssii.h | 273 +++++++++++++----- 19 files changed, 817 insertions(+), 206 deletions(-) create mode 100644 Auxiliary_utils/LibSidServo/examples/Readme.md create mode 100644 Auxiliary_utils/LibSidServo/examples/SSIIconf.c create mode 100644 Auxiliary_utils/LibSidServo/examples/conf.c create mode 100644 Auxiliary_utils/LibSidServo/examples/conf.h diff --git a/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt b/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt index 54128f0..944628d 100644 --- a/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt +++ b/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt @@ -5,8 +5,9 @@ include_directories(../) 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) +add_executable(goto goto.c dump.c conf.c) +add_executable(dump dumpmoving.c dump.c conf.c) +add_executable(dump_s dumpmoving_scmd.c dump.c conf.c) +add_executable(dumpswing dumpswing.c dump.c conf.c) +add_executable(traectory_s scmd_traectory.c dump.c traectories.c conf.c) +add_executable(SSIIconf SSIIconf.c conf.c) diff --git a/Auxiliary_utils/LibSidServo/examples/Readme.md b/Auxiliary_utils/LibSidServo/examples/Readme.md new file mode 100644 index 0000000..6a7e1dc --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/Readme.md @@ -0,0 +1,28 @@ +Some examples of usage of libsidservo +===================================== + +## Auxiliary files + +*conf.c*, *conf.h* - base configuration - read from file (default: servo.conf) - to simplify examples running when config changes + +*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. + +*SSIIconf.c* (`SSIIconf`) - read/write hardware configuration of controller + diff --git a/Auxiliary_utils/LibSidServo/examples/SSIIconf.c b/Auxiliary_utils/LibSidServo/examples/SSIIconf.c new file mode 100644 index 0000000..c5fb631 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/SSIIconf.c @@ -0,0 +1,76 @@ +/* + * 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 "conf.h" +#include "sidservo.h" +#include "simpleconv.h" + +typedef struct{ + int help; + int helpargs; + int writeconf; + char *conffile; + char *hwconffile; +} parameters; + +static hardware_configuration_t HW = {0}; + +static parameters G = {0}; + +static sl_option_t cmdlnopts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"help-opts", NO_ARGS, NULL, 'H', arg_int, APTR(&G.helpargs), "configuration help"}, + {"serconf", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "serial configuration file name"}, + {"hwconf", NEED_ARG, NULL, 'i', arg_string, APTR(&G.hwconffile),"SSII configuration file name"}, + {"writeconf", NO_ARGS, NULL, 0, arg_int, APTR(&G.writeconf), "write configuration (BE CAREFUL!)"}, + end_option +}; + +static sl_option_t confopts[] = { + {"Xaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Xconf.accel), "X Default Acceleration, rad/s^2"}, + {"Yaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Yconf.accel), "Y Default Acceleration, rad/s^2"}, + end_option +}; + +int main(int argc, char** argv){ + sl_init(); + sl_parseargs(&argc, &argv, cmdlnopts); + if(G.help) + sl_showhelp(-1, cmdlnopts); + if(G.helpargs) + sl_showhelp(-1, confopts); + conf_t *sconf = readServoConf(G.conffile); + if(!sconf){ + dumpConf(); + return 1; + } + if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount"); + if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration"); + char *c = sl_print_opts(confopts, TRUE); + green("Got configuration:\n"); + printf("%s\n", c); + FREE(c); + if(G.hwconffile && G.writeconf){ + ; + } + Mount.quit(); + return 0; +} diff --git a/Auxiliary_utils/LibSidServo/examples/conf.c b/Auxiliary_utils/LibSidServo/examples/conf.c new file mode 100644 index 0000000..428bec4 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/conf.c @@ -0,0 +1,65 @@ +/* + * 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 "conf.h" + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + .EncoderDevPath = "/dev/ttyUSB1", + .EncoderDevSpeed = 153000, + .MountReqInterval = 0.1, + .SepEncoder = 1 +}; + +static sl_option_t opts[] = { + {"MountDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.MountDevPath), "path to mount device"}, + {"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of mount device"}, + {"EncoderDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderDevPath), "path to encoder device"}, + {"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"}, + {"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"}, + {"SepEncoder", NO_ARGS, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device"}, + end_option +}; + +conf_t *readServoConf(const char *filename){ + if(!filename) filename = DEFCONFFILE; + int n = sl_conf_readopts(filename, opts); + if(n < 0){ + WARNX("Can't read file %s", filename); + return NULL; + } + if(n == 0){ + WARNX("Got ZERO parameters from %s", filename); + return NULL; + } + return &Config; +} + +void dumpConf(){ + char *c = sl_print_opts(opts, TRUE); + printf("Current configuration:\n%s\n", c); + FREE(c); +} + +void confHelp(){ + sl_showhelp(-1, opts); +} diff --git a/Auxiliary_utils/LibSidServo/examples/conf.h b/Auxiliary_utils/LibSidServo/examples/conf.h new file mode 100644 index 0000000..f15d4ab --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/conf.h @@ -0,0 +1,27 @@ +/* + * 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" + +#define DEFCONFFILE "servo.conf" + +void confHelp(); +conf_t *readServoConf(const char *filename); +void dumpConf(); diff --git a/Auxiliary_utils/LibSidServo/examples/dump.c b/Auxiliary_utils/LibSidServo/examples/dump.c index 4b57543..604d1ca 100644 --- a/Auxiliary_utils/LibSidServo/examples/dump.c +++ b/Auxiliary_utils/LibSidServo/examples/dump.c @@ -36,8 +36,8 @@ void logmnt(FILE *fcoords, mountdata_t *m){ fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n"); return; } - if(t0 < 0.) t0 = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6; - double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6 - t0; + if(t0 < 0.) t0 = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6; + double t = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6 - t0; // write data fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10u %6.1f %4.1f\n", t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y), @@ -64,17 +64,17 @@ void dumpmoving(FILE *fcoords, double t, int N){ WARNX("Can't get mount data"); LOGWARN("Can't get mount data"); } - uint32_t millis = mdata.millis; + uint32_t millis = mdata.encposition.msrtime.tv_usec; int ctr = -1; double xlast = mdata.motposition.X, ylast = mdata.motposition.Y; double t0 = sl_dtime(); //DBG("millis = %u", millis); while(sl_dtime() - t0 < t && ctr < N){ - usleep(10000); + usleep(1000); if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} - if(mdata.millis == millis) continue; + if(mdata.encposition.msrtime.tv_usec == millis) continue; //DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y); - millis = mdata.millis; + millis = mdata.encposition.msrtime.tv_usec; if(fcoords) logmnt(fcoords, &mdata); if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ xlast = mdata.motposition.X; @@ -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/Auxiliary_utils/LibSidServo/examples/dumpmoving.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving.c index e08b80e..323e310 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpmoving.c +++ b/Auxiliary_utils/LibSidServo/examples/dumpmoving.c @@ -24,6 +24,7 @@ #include #include +#include "conf.h" #include "dump.h" #include "sidservo.h" #include "simpleconv.h" @@ -34,6 +35,7 @@ typedef struct{ int Ncycles; char *logfile; char *coordsoutput; + char *conffile; } parameters; static parameters G = { @@ -47,6 +49,7 @@ static sl_option_t cmdlnopts[] = { {"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), "log file name"}, {"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"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, end_option }; @@ -60,15 +63,6 @@ void signals(int sig){ exit(sig); } -static conf_t Config = { - .MountDevPath = "/dev/ttyUSB0", - .MountDevSpeed = 19200, - //.EncoderDevPath = "/dev/ttyUSB1", - //.EncoderDevSpeed = 153000, - .MountReqInterval = 0.1, - .SepEncoder = 0 -}; - int main(int argc, char **argv){ sl_init(); sl_parseargs(&argc, &argv, cmdlnopts); @@ -76,6 +70,11 @@ int main(int argc, char **argv){ sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR; if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1; if(G.logfile) OPENLOG(G.logfile, lvl, 1); + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + return 1; + } if(G.coordsoutput){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); @@ -83,22 +82,21 @@ int main(int argc, char **argv){ logmnt(fcoords, NULL); time_t curtime = time(NULL); LOGMSG("Started @ %s", ctime(&curtime)); - LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed); - LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed); - mcc_errcodes_t e = Mount.init(&Config); - if(e != MCC_E_OK){ - WARNX("Can't init devices"); - return 1; - } + LOGMSG("Mount device %s @ %d", Config->MountDevPath, Config->MountDevSpeed); + LOGMSG("Encoder device %s @ %d", Config->EncoderDevPath, Config->EncoderDevSpeed); + if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init devices"); + coords_t M; + 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 signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z - if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.))) + double tagx = DEG2RAD(45.) + M.X, tagy = DEG2RAD(45.) + M.Y; + if(MCC_E_OK != Mount.moveTo(&tagx, &tagy)) ERRX("Can't move to 45, 45"); dumpmoving(fcoords, 30., G.Ncycles); - Mount.moveTo(0., 0.); + Mount.moveTo(&M.X, &M.Y); dumpmoving(fcoords, 30., G.Ncycles); signals(0); return 0; diff --git a/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c index 2adedd3..ec9a029 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c +++ b/Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c @@ -26,6 +26,7 @@ #include #include +#include "conf.h" #include "dump.h" #include "sidservo.h" #include "simpleconv.h" @@ -33,24 +34,31 @@ typedef struct{ int help; int Ncycles; + int relative; double reqint; char *coordsoutput; + char *conffile; char *axis; } parameters; static parameters G = { .Ncycles = 40, - .reqint = 0.1, + .reqint = -1., .axis = "X", }; + static FILE *fcoords = NULL; +static coords_t M; + 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)"}, - {"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"}, + {"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X, Y or B for both)"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, + {"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"}, end_option }; @@ -63,15 +71,6 @@ void signals(int sig){ exit(sig); } -static conf_t Config = { - .MountDevPath = "/dev/ttyUSB0", - .MountDevSpeed = 19200, - //.EncoderDevPath = "/dev/ttyUSB1", - //.EncoderDevSpeed = 153000, - .MountReqInterval = 0.1, - .SepEncoder = 0 -}; - // dump thread static void *dumping(void _U_ *u){ dumpmoving(fcoords, 3600., G.Ncycles); @@ -108,20 +107,23 @@ 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); + if(*G.axis == 'X' || *G.axis == 'B'){ + cmd.Xmot = DEG2RAD(target) + M.X; cmd.Xspeed = DEG2RAD(speed); - }else{ - cmd.Ymot = DEG2RAD(target); + limit = DEG2RAD(limit) + M.X; + } + if(*G.axis == 'Y' || *G.axis == 'B'){ + 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 } @@ -130,20 +132,25 @@ 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"); + if(strcmp(G.axis, "X") && strcmp(G.axis, "Y") && strcmp(G.axis, "B")){ + WARNX("\"Axis\" should be X, Y or B"); return 1; } if(G.coordsoutput){ if(!(fcoords = fopen(G.coordsoutput, "w"))) 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){ + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + return 1; + } + if(G.reqint > 0.) Config->MountReqInterval = G.reqint; + 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 +158,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 +171,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/Auxiliary_utils/LibSidServo/examples/dumpswing.c b/Auxiliary_utils/LibSidServo/examples/dumpswing.c index b2bbba0..73c50e4 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpswing.c +++ b/Auxiliary_utils/LibSidServo/examples/dumpswing.c @@ -22,6 +22,7 @@ #include #include +#include "conf.h" #include "dump.h" #include "sidservo.h" #include "simpleconv.h" @@ -35,6 +36,7 @@ typedef struct{ double period; double amplitude; char *coordsoutput; + char *conffile; char *axis; } parameters; @@ -55,6 +57,7 @@ static sl_option_t cmdlnopts[] = { {"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)"}, + {"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"}, end_option }; @@ -67,15 +70,6 @@ void signals(int sig){ 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); @@ -125,7 +119,12 @@ int main(int argc, char **argv){ 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); + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + return 1; + } + mcc_errcodes_t e = Mount.init(Config); if(e != MCC_E_OK){ WARNX("Can't init devices"); return 1; @@ -147,23 +146,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/Auxiliary_utils/LibSidServo/examples/goto.c b/Auxiliary_utils/LibSidServo/examples/goto.c index 18ebab0..3da2b9a 100644 --- a/Auxiliary_utils/LibSidServo/examples/goto.c +++ b/Auxiliary_utils/LibSidServo/examples/goto.c @@ -23,6 +23,7 @@ #include #include +#include "conf.h" #include "dump.h" #include "sidservo.h" #include "simpleconv.h" @@ -31,7 +32,9 @@ typedef struct{ int help; int Ncycles; int wait; + int relative; char *coordsoutput; + char *conffile; double X; double Y; } parameters; @@ -49,18 +52,11 @@ 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"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, end_option }; -static conf_t Config = { - .MountDevPath = "/dev/ttyUSB0", - .MountDevSpeed = 19200, - //.EncoderDevPath = "/dev/ttyUSB1", - //.EncoderDevSpeed = 153000, - .MountReqInterval = 0.1, - .SepEncoder = 0 -}; - static FILE* fcoords = NULL; static pthread_t dthr; @@ -84,8 +80,14 @@ static void *dumping(void _U_ *u){ int main(int _U_ argc, char _U_ **argv){ sl_init(); sl_parseargs(&argc, &argv, cmdlnopts); - if(G.help) sl_showhelp(-1, cmdlnopts); - if(MCC_E_OK != Mount.init(&Config)) ERRX("Can't init mount"); + if(G.help) + sl_showhelp(-1, cmdlnopts); + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + return 1; + } + 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){ @@ -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); @@ -112,6 +126,9 @@ int main(int _U_ argc, char _U_ **argv){ } out: if(G.coordsoutput) pthread_join(dthr, NULL); - if(G.wait) Mount.quit(); + if(G.wait){ + if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y)); + Mount.quit(); + } return 0; } diff --git a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c index 19ae664..00dacd2 100644 --- a/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c +++ b/Auxiliary_utils/LibSidServo/examples/scmd_traectory.c @@ -20,6 +20,7 @@ #include #include +#include "conf.h" #include "dump.h" #include "sidservo.h" #include "simpleconv.h" @@ -38,6 +39,7 @@ typedef struct{ double Y0; // -//- char *coordsoutput; // dump file char *tfn; // traectory function name + char *conffile; } parameters; static FILE *fcoords = NULL; @@ -64,18 +66,10 @@ static sl_option_t cmdlnopts[] = { {"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)"}, + {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, 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){ @@ -127,7 +121,12 @@ int main(int argc, char **argv){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); }else fcoords = stdout; - Config.MountReqInterval = G.reqint; + conf_t *Config = readServoConf(G.conffile); + if(!Config){ + dumpConf(); + return 1; + } + 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); @@ -139,7 +138,7 @@ int main(int argc, char **argv){ ERRX("Can't init traectory"); return 1; } - mcc_errcodes_t e = Mount.init(&Config); + mcc_errcodes_t e = Mount.init(Config); if(e != MCC_E_OK){ WARNX("Can't init devices"); return 1; diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user index 524f200..dff1f3d 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/Auxiliary_utils/LibSidServo/libsidservo.files b/Auxiliary_utils/LibSidServo/libsidservo.files index 4adc54f..18d928c 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.files +++ b/Auxiliary_utils/LibSidServo/libsidservo.files @@ -1,5 +1,8 @@ CMakeLists.txt dbg.h +examples/SSIIconf.c +examples/conf.c +examples/conf.h examples/dump.c examples/dump.h examples/dumpmoving.c diff --git a/Auxiliary_utils/LibSidServo/main.c b/Auxiliary_utils/LibSidServo/main.c index e7b4a20..788ad86 100644 --- a/Auxiliary_utils/LibSidServo/main.c +++ b/Auxiliary_utils/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"); } @@ -61,26 +62,101 @@ static mcc_errcodes_t init(conf_t *c){ ret = MCC_E_ENCODERDEV; } } - if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.001){ + if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){ DBG("Bad value of MountReqInterval"); ret = MCC_E_BADFORMAT; } + uint8_t buf[1024]; + data_t d = {.buf = buf, .len = 0, .maxlen = 1024}; + // read input data as there may be some trash on start + if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED; if(ret != MCC_E_OK) quit(); 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; + int32_t tag = X_RAD2MOT(*X); + DBG("X: %g, tag: %d", *X, tag); + 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; + int32_t tag = Y_RAD2MOT(*Y); + DBG("Y: %g, tag: %d", *Y, tag); + 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; } @@ -89,7 +165,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; } @@ -149,13 +230,34 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){ return MCC_E_OK; } +mcc_errcodes_t get_hwconf(hardware_configuration_t *c){ + if(!c) return MCC_E_BADFORMAT; + SSconfig conf; + if(!cmdC(&conf, FALSE)) return MCC_E_FAILED; + // and bored transformations + DBG("Xacc=%u", conf.Xconf.accel); + DBG("Yacc=%u", conf.Yconf.accel); + c->Xconf.accel = X_MOTACC2RS(conf.Xconf.accel); + DBG("cacc: %g", c->Xconf.accel); + c->Xconf.backlash = conf.Xconf.backlash; + // ... + c->Yconf.accel = X_MOTACC2RS(conf.Yconf.accel); + c->Xconf.backlash = conf.Xconf.backlash; + // ... + return MCC_E_OK; +} + // init mount class mount_t Mount = { .init = init, .quit = quit, .getMountData = getMD, .moveTo = move2, + .moveWspeed = move2s, + .setSpeed = setspeed, .emergStop = emstop, + .stop = stop, .shortCmd = shortcmd, .longCmd = longcmd, + .getHWconfig = get_hwconf, }; diff --git a/Auxiliary_utils/LibSidServo/serial.c b/Auxiliary_utils/LibSidServo/serial.c index 7fc46d1..3f4b823 100644 --- a/Auxiliary_utils/LibSidServo/serial.c +++ b/Auxiliary_utils/LibSidServo/serial.c @@ -48,8 +48,8 @@ static struct timeval encRtmout = {0}, mntRtmout = {0}; // encoders raw data typedef struct __attribute__((packed)){ uint8_t magick; - int32_t encX; int32_t encY; + int32_t encX; uint8_t CRC[4]; } enc_t; @@ -82,6 +82,13 @@ static void gttime(){ */ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){ enc_t *edata = (enc_t*) databuf; +/* +#ifdef EBUG + DBG("ENCBUF:"); + for(int i = 0; i < ENC_DATALEN; ++i) printf("%02X ", databuf[i]); + printf("\n"); +#endif +*/ if(edata->magick != ENC_MAGICK){ DBG("No magick"); return; @@ -112,13 +119,13 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){ mountdata.encposition.Y = Y_ENC2RAD(edata->encY); mountdata.encposition.msrtime = *tv; pthread_mutex_unlock(&datamutex); - DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI); + //DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI); } // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected static int getencbyte(){ if(encfd < 0) return -1; - uint8_t byte; + uint8_t byte = 0; fd_set rfds; struct timeval tv; do{ @@ -181,10 +188,10 @@ static void *encoderthread(void _U_ *u){ if(b == -2) ++errctr; if(b < 0) continue; errctr = 0; - DBG("Got byte from Encoder: 0x%02X", b); +// DBG("Got byte from Encoder: 0x%02X", b); if(wridx == 0){ if((uint8_t)b == ENC_MAGICK){ - DBG("Got magic -> start filling packet"); +// DBG("Got magic -> start filling packet"); databuf[wridx++] = (uint8_t) b; gettimeofday(&tv, NULL); } @@ -239,7 +246,11 @@ static void *mountthread(void _U_ *u){ struct timeval tgot; if(0 != gettimeofday(&tgot, NULL)) continue; if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){ +#ifdef EBUG DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len); + for(size_t i = 0; i < d.len; ++i) printf("%02X ", d.buf[i]); + printf("\n"); +#endif ++errctr; continue; } if(SScalcChecksum(buf, sizeof(SSstat)-2) != status->checksum){ @@ -375,13 +386,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; } @@ -398,6 +410,13 @@ int MountWriteRead(const data_t *out, data_t *in){ pthread_mutex_unlock(&mntmutex); return ret; } +// send binary data - without EOL +int MountWriteReadRaw(const data_t *out, data_t *in){ + pthread_mutex_lock(&mntmutex); + int ret = wr(out, in, 0); + pthread_mutex_unlock(&mntmutex); + return ret; +} #ifdef EBUG static void logscmd(SSscmd *c){ @@ -459,3 +478,34 @@ int cmdS(SSscmd *cmd){ int cmdL(SSlcmd *cmd){ return bincmd((uint8_t *)cmd, sizeof(SSlcmd)); } +// rw == 1 to write, 0 to read +int cmdC(SSconfig *conf, int rw){ + static data_t *wcmd = NULL, *rcmd = NULL; + int ret = FALSE; + // dummy buffer to clear trash in input + char ans[300]; + data_t a = {.buf = (uint8_t*)ans, .maxlen=299}; + if(!wcmd) wcmd = cmd2dat(CMD_PROGFLASH); + if(!rcmd) rcmd = cmd2dat(CMD_DUMPFLASH); + pthread_mutex_lock(&mntmutex); + if(rw){ // write + if(!wr(wcmd, &a, 1)) goto rtn; + }else{ // read + data_t d; + d.buf = (uint8_t *) conf; + d.len = 0; d.maxlen = sizeof(SSconfig); + ret = wr(rcmd, &d, 1); + DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen); + if(d.len != d.maxlen) return FALSE; + // simplest checksum + uint16_t sum = 0; + for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i]; + if(sum != conf->checksum){ + DBG("got sum: %u, need: %u", conf->checksum, sum); + return FALSE; + } + } +rtn: + pthread_mutex_unlock(&mntmutex); + return ret; +} diff --git a/Auxiliary_utils/LibSidServo/serial.h b/Auxiliary_utils/LibSidServo/serial.h index d92542b..aa2604c 100644 --- a/Auxiliary_utils/LibSidServo/serial.h +++ b/Auxiliary_utils/LibSidServo/serial.h @@ -36,5 +36,7 @@ int openMount(const char *path, int speed); void closeSerial(); mcc_errcodes_t getMD(mountdata_t *d); int MountWriteRead(const data_t *out, data_t *in); +int MountWriteReadRaw(const data_t *out, data_t *in); int cmdS(SSscmd *cmd); int cmdL(SSlcmd *cmd); +int cmdC(SSconfig *conf, int rw); diff --git a/Auxiliary_utils/LibSidServo/sidservo.h b/Auxiliary_utils/LibSidServo/sidservo.h index 79ea016..5f85d50 100644 --- a/Auxiliary_utils/LibSidServo/sidservo.h +++ b/Auxiliary_utils/LibSidServo/sidservo.h @@ -18,6 +18,11 @@ #pragma once +#ifdef __cplusplus +extern "C" +{ +#endif + #include #include #include @@ -55,8 +60,30 @@ typedef struct{ } data_t; typedef struct{ - uint8_t XBits; - uint8_t YBits; + uint8_t motrev :1; // If 1, the motor encoder is incremented in the opposite direction + uint8_t motpolarity :1; // If 1, the motor polarity is reversed + uint8_t encrev :1; // If 1, the axis encoder is reversed + uint8_t dragtrack :1; // If 1, we are in computerless Drag and Track mode + uint8_t trackplat :1; // If 1, we are in the tracking platform mode + uint8_t handpaden :1; // If 1, hand paddle is enabled + uint8_t newpad :1; // If 1, hand paddle is compatible with New hand paddle, which allows slewing in two directions and guiding + uint8_t guidemode :1; // If 1, we are in guide mode. The pan rate is added or subtracted from the current tracking rate +} xbits_t; + +typedef struct{ + uint8_t motrev :1; // If 1, the motor encoder is incremented in the opposite direction + uint8_t motpolarity :1; // If 1, the motor polarity is reversed + uint8_t encrev :1; // If 1, the axis encoder is reversed + /* If 1, we are in computerless Slew and Track mode + (no clutches; use handpad to slew; must be in Drag and Track mode too) */ + uint8_t slewtrack :1; + uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync + uint8_t digin :3; // Digital input from radio handpad receiver +} ybits_t; + +typedef struct{ + xbits_t XBits; + ybits_t YBits; uint8_t ExtraBits; uint16_t ain0; uint16_t ain1; @@ -71,8 +98,16 @@ typedef struct{ uint32_t millis; double temperature; double voltage; + int32_t XmotRaw; + int32_t YmotRaw; + int32_t XencRaw; + int32_t YencRaw; } mountdata_t; +typedef struct{ + ; +} mountstat_t; + typedef struct{ double Xmot; // 0 X motor position (rad) double Xspeed; // 4 X speed (rad/s) @@ -94,15 +129,64 @@ typedef struct{ double Yatime; // 28 } long_command_t; // long command +// hardware axe configuration +typedef struct{ + double accel; // Default Acceleration, rad/s^2 + double backlash; // Backlash (???) + double errlimit; // Error Limit, rad + double propgain; // Proportional Gain (???) + double intgain; // Integral Gain (???) + double derivgain; // Derivative Gain (???) + double outplimit; // Output Limit, percent (0..100) + double currlimit; // Current Limit (A) + double intlimit; // Integral Limit (???) +} __attribute__((packed)) axe_config_t; + +// hardware configuration +typedef struct{ + axe_config_t Xconf; + xbits_t xbits; + axe_config_t Yconf; + ybits_t ybits; + uint8_t address; + double eqrate; // Equatorial Rate (???) + double eqadj; // Equatorial UpDown adjust (???) + double trackgoal; // Tracking Platform Goal (???) + double latitude; // Latitude, rad + uint32_t Ysetpr; // Azm Scope Encoder Ticks Per Rev + uint32_t Xsetpr; // Alt Scope Encoder Ticks Per Rev + uint32_t Ymetpr; // Azm Motor Ticks Per Rev + uint32_t Xmetpr; // Alt Motor Ticks Per Rev + double Xslewrate; // Alt/Dec Slew Rate (rad/s) + double Yslewrate; // Azm/RA Slew Rate (rad/s) + double Xpanrate; // Alt/Dec Pan Rate (rad/s) + double Ypanrate; // Azm/RA Pan Rate (rad/s) + double Xguiderate; // Alt/Dec Guide Rate (rad/s) + double Yguiderate; // Azm/RA Guide Rate (rad/s) + uint32_t baudrate; // Baud Rate (baud) + double locsdeg; // Local Search Degrees (rad) + double locsspeed; // Local Search Speed (rad/s) + double backlspd; // Backlash speed (???) +} hardware_configuration_t; + // mount class 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 + mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration } mount_t; extern mount_t Mount; + + +#ifdef __cplusplus +} +#endif diff --git a/Auxiliary_utils/LibSidServo/ssii.c b/Auxiliary_utils/LibSidServo/ssii.c index 9a008b3..6a04241 100644 --- a/Auxiliary_utils/LibSidServo/ssii.c +++ b/Auxiliary_utils/LibSidServo/ssii.c @@ -90,6 +90,19 @@ int SStextcmd(const char *cmd, data_t *answer){ return MountWriteRead(&d, answer); } +// the same as SStextcmd, but not adding EOL - send raw 'cmd' +int SSrawcmd(const char *cmd, data_t *answer){ + if(!cmd){ + DBG("try to send empty command"); + return FALSE; + } + data_t d; + d.buf = (uint8_t*) cmd; + d.len = d.maxlen = strlen(cmd); + DBG("send %zd bytes: %s", d.len, d.buf); + return MountWriteReadRaw(&d, answer); +} + /** * @brief SSgetint - send text command and return integer answer * @param cmd (i) - command to send @@ -116,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%" PRIi32, 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/Auxiliary_utils/LibSidServo/ssii.h b/Auxiliary_utils/LibSidServo/ssii.h index 9250dd9..866f78f 100644 --- a/Auxiliary_utils/LibSidServo/ssii.h +++ b/Auxiliary_utils/LibSidServo/ssii.h @@ -23,92 +23,186 @@ #include "sidservo.h" -#if 0 -// ASCII commands -#define U8P(x) ((uint8_t*)x) -// get binary data of all statistics -#define CMD_GETSTAT U8P("XXS") -// send short command -#define CMD_SHORTCMD U8P("XXR") -// send long command -#define CMD_LONGCMD U8P("YXR") +/*********** base commands ***********/ // get/set X/Y in motsteps -#define CMD_MOTX U8P("X") -#define CMD_MOTY U8P("Y") -// -//- in encoders' ticks -#define CMD_ENCX U8P("XZ") -#define CMD_ENCY U8P("YZ") +#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_MOTXYS "S" +// reset current motor position to given value (and stop, if moving) +#define CMD_MOTXSET "XF" +#define CMD_MOTYSET "YF" +// acceleration (per each loop, max: 3900) +#define CMD_MOTXACCEL "XR" +#define CMD_MOTYACCEL "YR" +// PID regulator: +// P: 0..32767 +#define CMD_PIDPX "XP" +#define CMD_PIDPY "YP" +// I: 0..32767 +#define CMD_PIDIX "XI" +#define CMD_PIDIY "YI" +// limit of I (doesn't work): 0:24000 (WTF???) +#define CMD_PIDILX "XL" +#define CMD_PIDILY "YL" +// D: 0..32767 +#define CMD_PIDDX "XD" +#define CMD_PIDDY "YD" +// current position error +#define CMD_POSERRX "XE" +#define CMD_POSERRY "YE" +// max position error limit (X: E#, Y: e#) +#define CMD_POSERRLIMX "XEL" +#define CMD_POSERRLIMY "YEL" +// current PWM output: 0..255 (or set max PWM out) +#define CMD_PWMOUTX "XO" +#define CMD_PWMOUTY "YO" +// motor current *100 (or set current limit): 0..240 +#define CMD_MOTCURNTX "XC" +#define CMD_MOTCURNTY "YC" +// change axis to Manual mode and set the PWM output: -255:255 +#define CMD_MANUALPWMX "XM" +#define CMD_MANUALPWMY "YM" +// change axis to Auto mode +#define CMD_AUTOX "XA" +#define CMD_AUTOY "YA" +// get positioin in encoders' ticks or reset it to given value +#define CMD_ENCX "XZ" +#define CMD_ENCY "YZ" +// get/set speed (geter x: S#, getter y: s#) +#define CMD_SPEEDX "XS" +#define CMD_SPEEDY "YS" // normal stop X/Y -#define CMD_STOPX U8P("XN") -#define CMD_STOPY U8P("YN") +#define CMD_STOPX "XN" +#define CMD_STOPY "YN" +// lower speed -> drag&track or slew&track +#define CMD_STOPTRACKX "XNT" +#define CMD_STOPTRACKY "YNT" // emergency stop -#define CMD_EMSTOPX U8P("XG") -#define CMD_EMSTOPY U8P("YG") -// getters of motor's encoders per rev -#define CMD_GETXMEPR U8P("XXU") -#define CMD_GETYMEPR U8P("XXV") +#define CMD_EMSTOPX "XG" +#define CMD_EMSTOPY "YG" +// get/set X/Ybits +#define CMD_BITSX "XB" +#define CMD_BITSY "YB" + +/*********** getters/setters without "Y" variant ***********/ +// get handpad status (decimal) +#define CMD_HANDPAD "XK" +// get TCPU (deg F) +#define CMD_TCPU "XH" +// get firmware version *10 +#define CMD_FIRMVER "XV" +// get motor voltage *10 +#define CMD_MOTVOLTAGE "XJ" +// get/set current CPU clock (milliseconds) +#define CMD_MILLIS "XY" +// reset servo +#define CMD_RESET "XQ" +// clear to factory defaults +#define CMD_CLRDEFAULTS "XU" +// save configuration to flash ROM +#define CMD_WRITEFLASH "XW" +// read config from flash to RAM +#define CMD_READFLASH "XT" +// write to flash following full config (128 bytes + 2 bytes of checksum) +#define CMD_PROGFLASH "FC" +// read configuration (-//-) +#define CMD_DUMPFLASH "SC" +// get serial number +#define CMD_SERIAL "YV" + +/*********** extended commands ***********/ +// get/set latitute +#define CMD_LATITUDE "XXL" +// getters/setters of motor's encoders per rev +#define CMD_MEPRX "XXU" +#define CMD_MEPRY "XXV" // -//- axis encoders -#define CMD_GETXAEPR U8P("XXT") -#define CMD_GETYAEPR U8P("XXZ") -// exit ASCII checksum mode -#define CMD_EXITACM U8P("YXY0\r\xb8") -#endif +#define CMD_AEPRX "XXT" +#define CMD_AEPRY "XXZ" +// get/set slew rate +#define CMD_SLEWRATEX "XXA" +#define CMD_SLEWRATEY "XXB" +// get/set pan rate +#define CMD_PANRATEX "XXC" +#define CMD_PANRATEY "XXD" +// get/set platform tracking rate +#define CMD_PLATRATE "XXE" +// get/set platform up/down adjuster +#define CMD_PLATADJ "XXF" +// get/set platform goal +#define CMD_PLATGOAL "XXG" +// get/set guide rate +#define CMD_GUIDERATEX "XXH" +#define CMD_GUIDERATEY "XXI" +// get/set picservo timeout (seconds) +#define CMD_PICTMOUT "XXJ" +// get/set digital outputs of radio handpad +#define CMD_RADIODIGOUT "XXQ" +// get/set argo navis mode +#define CMD_ARGONAVIS "XXN" +// get/set local search distance +#define CMD_LOSCRCHDISTX "XXM" +#define CMD_LOSCRCHDISTY "XXO" +// get/set backlash +#define CMD_BACKLASHX "XXO" +#define CMD_BACKLASHY "XXP" + // get binary data of all statistics -#define CMD_GETSTAT ("XXS") +#define CMD_GETSTAT "XXS" // send short command -#define CMD_SHORTCMD ("XXR") +#define CMD_SHORTCMD "XXR" // send long command -#define CMD_LONGCMD ("YXR") -// get/set X/Y in motsteps -#define CMD_MOTX ("X") -#define CMD_MOTY ("Y") -// -//- in encoders' ticks -#define CMD_ENCX ("XZ") -#define CMD_ENCY ("YZ") -// normal stop X/Y -#define CMD_STOPX ("XN") -#define CMD_STOPY ("YN") -// emergency stop -#define CMD_EMSTOPX ("XG") -#define CMD_EMSTOPY ("YG") -// getters of motor's encoders per rev -#define CMD_GETXMEPR ("XXU") -#define CMD_GETYMEPR ("XXV") -// -//- axis encoders -#define CMD_GETXAEPR ("XXT") -#define CMD_GETYAEPR ("XXZ") +#define CMD_LONGCMD "YXR" + + +/*********** special ***********/ // exit ASCII checksum mode -#define CMD_EXITACM ("YXY0\r\xb8") +#define CMD_EXITACM "YXY0\r\xb8" +// controller status: +// X# Y# XZ# YZ# XC# YC# V# T# X[AM] Y[AM] K# +// X,Y - motor, XZ,YZ - encoder, XC,YC - current*100, V - voltage*10, T - temp (F), XA,YA - mode (A[uto]/M[anual]), K - handpad status bits +#define CMD_GETSTATTEXT "\r" + + // steps per revolution -//#define X_MOT_STEPSPERREV (3325440.) #define X_MOT_STEPSPERREV (3325952.) -//#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) -#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)) +#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) ((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) ((int32_t)(X_RAD2MOT(r)*65536./1953.)) #define Y_MOTSPD2RS(n) (Y_MOT2RAD(n)/65536.*1953.) #define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953.)) +// motor acceleration -//- +#define X_MOTACC2RS(n) (X_MOT2RAD(n)/65536.*1953.*1953.) +#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r)*65536./1953./1953.)) +#define Y_MOTACC2RS(n) (Y_MOT2RAD(n)/65536.*1953.*1953.) +#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953./1953.)) + // adder time to seconds vice versa -#define ADDER2S(a) (a*1953.) -#define S2ADDER(s) (s/1953.) +#define ADDER2S(a) ((a)*1953.) +#define S2ADDER(s) ((s)/1953.) // encoder per revolution #define X_ENC_STEPSPERREV (67108864.) #define Y_ENC_STEPSPERREV (67108864.) // encoder position to radians and back -#define X_ENC2RAD(n) (2.*M_PI * (double)n / X_ENC_STEPSPERREV) -#define Y_ENC2RAD(n) (2.*M_PI * (double)n / Y_ENC_STEPSPERREV) -#define X_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * X_ENC_STEPSPERREV)) -#define Y_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * Y_ENC_STEPSPERREV)) +#define X_ENC2RAD(n) (2.*M_PI * ((double)n) / X_ENC_STEPSPERREV) +#define Y_ENC2RAD(n) (2.*M_PI * ((double)n) / Y_ENC_STEPSPERREV) +#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV)) +#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV)) // encoder's tolerance (ticks) #define YencTOL (25.) @@ -123,8 +217,8 @@ typedef struct{ // 41 bytes int32_t Xenc; // 9 Dec/HA encoder position int32_t Yenc; // 13 uint8_t keypad; // 17 keypad status - uint8_t XBits; // 18 - uint8_t YBits; // 19 + xbits_t XBits; // 18 + ybits_t YBits; // 19 uint8_t ExtraBits; // 20 uint16_t ain0; // 21 analog inputs uint16_t ain1; // 23 @@ -159,11 +253,60 @@ typedef struct{ uint16_t checksum; // 32 } __attribute__((packed)) SSlcmd; // long command +typedef struct{ + uint32_t accel; // Default Acceleration (0..3900) + uint32_t backlash; // Backlash (???) + uint16_t errlimit; // Error Limit (0..32767) + uint16_t propgain; // Proportional Gain (0..32767) + uint16_t intgain; // Integral Gain (0..32767) + uint16_t derivgain; // Derivative Gain (0..32767) + uint16_t outplimit; // Output Limit, 0xFF = 100.0 (0..255) + uint16_t currlimit; // Current Limit * 100 (0..240) + uint16_t intlimit; // Integral Limit (0..24000) +} __attribute__((packed)) AxeConfig; + +typedef struct{ + AxeConfig Xconf; + xbits_t xbits; + uint8_t unused0; + AxeConfig Yconf; + ybits_t ybits; + uint8_t unused1; + uint8_t address; + uint8_t unused2; + uint32_t eqrate; // Equatorial Rate (platform?) (???) + int32_t eqadj; // Equatorial UpDown adjust (???) + uint32_t trackgoal; // Tracking Platform Goal (???) + uint16_t latitude; // Latitude * 100, MSB FIRST!! + uint32_t Ysetpr; // Azm Scope Encoder Ticks Per Rev, MSB FIRST!! + uint32_t Xsetpr; // Alt Scope Encoder Ticks Per Rev, MSB FIRST!! + uint32_t Ymetpr; // Azm Motor Ticks Per Rev, MSB FIRST!! + uint32_t Xmetpr; // Alt Motor Ticks Per Rev, MSB FIRST!! + int32_t Xslewrate; // Alt/Dec Slew Rate (rates are negative in "through the pole" mode!!!) + int32_t Yslewrate; // Azm/RA Slew Rate + int32_t Xpanrate; // Alt/Dec Pan Rate + int32_t Ypanrate; // Azm/RA Pan Rate + int32_t Xguiderate; // Alt/Dec Guide Rate + int32_t Yguiderate; // Azm/RA Guide Rate + uint8_t unknown0; // R/A PEC Auto Sync Enable (if low bit = 1), or PicServo Comm Timeout?? + uint8_t unused3; + uint8_t baudrate; // Baud Rate?? + uint8_t unused4; + uint8_t specmode; // 1 = Enable Argo Navis, 2 = Enable Sky Commander + uint8_t unused5; + uint32_t locsdeg; // Local Search Degrees * 100 + uint32_t locsspeed; // Local Search Speed, arcsec per sec (???) + uint32_t backlspd; // Backlash speed + uint32_t pecticks; // RA/Azm PEC Ticks + uint16_t unused6; + uint16_t checksum; +} __attribute__((packed)) SSconfig; + uint16_t SScalcChecksum(uint8_t *buf, int len); void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *tdat); 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);