From 5b89a968eb1d4c965d321e5062a3f8cde2be9d7a Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Tue, 25 Feb 2025 23:32:51 +0300 Subject: [PATCH] shit happens --- LibSidServo/examples/CMakeLists.txt | 10 ++-- LibSidServo/examples/Readme.md | 2 + LibSidServo/examples/conf.c | 65 ++++++++++++++++++++++++++ LibSidServo/examples/conf.h | 27 +++++++++++ LibSidServo/examples/dump.c | 12 ++--- LibSidServo/examples/dumpmoving.c | 35 +++++++------- LibSidServo/examples/dumpmoving_scmd.c | 41 ++++++++-------- LibSidServo/examples/dumpswing.c | 19 ++++---- LibSidServo/examples/goto.c | 27 ++++++----- LibSidServo/examples/scmd_traectory.c | 21 ++++----- LibSidServo/libsidservo.creator.user | 2 +- LibSidServo/libsidservo.files | 2 + LibSidServo/main.c | 6 ++- LibSidServo/serial.c | 19 ++++++-- LibSidServo/sidservo.h | 4 ++ LibSidServo/ssii.c | 2 +- LibSidServo/ssii.h | 22 ++++----- 17 files changed, 214 insertions(+), 102 deletions(-) create mode 100644 LibSidServo/examples/conf.c create mode 100644 LibSidServo/examples/conf.h diff --git a/LibSidServo/examples/CMakeLists.txt b/LibSidServo/examples/CMakeLists.txt index 54128f0..a8f89af 100644 --- a/LibSidServo/examples/CMakeLists.txt +++ b/LibSidServo/examples/CMakeLists.txt @@ -5,8 +5,8 @@ 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) diff --git a/LibSidServo/examples/Readme.md b/LibSidServo/examples/Readme.md index f4eefec..5204710 100644 --- a/LibSidServo/examples/Readme.md +++ b/LibSidServo/examples/Readme.md @@ -3,6 +3,8 @@ 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. diff --git a/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c new file mode 100644 index 0000000..428bec4 --- /dev/null +++ b/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/LibSidServo/examples/conf.h b/LibSidServo/examples/conf.h new file mode 100644 index 0000000..f15d4ab --- /dev/null +++ b/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/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c index 4b0c581..604d1ca 100644 --- a/LibSidServo/examples/dump.c +++ b/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; diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index 2769529..afff7f8 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/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_int, 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,23 +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 - double tag = DEG2RAD(45.), zero = 0.; - if(MCC_E_OK != Mount.moveTo(&tag, &tag)) + 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(&zero, &zero); + Mount.moveTo(&M.X, &M.Y); dumpmoving(fcoords, 30., G.Ncycles); signals(0); return 0; diff --git a/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c index a3f23bc..36e2d09 100644 --- a/LibSidServo/examples/dumpmoving_scmd.c +++ b/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_int, APTR(&G.conffile), "configuration file name"}, + {"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"}, end_option }; @@ -63,17 +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 -}; - -static coords_t M; - // dump thread static void *dumping(void _U_ *u){ dumpmoving(fcoords, 3600., G.Ncycles); @@ -115,11 +112,12 @@ 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'){ + if(*G.axis == 'X' || *G.axis == 'B'){ cmd.Xmot = DEG2RAD(target) + M.X; cmd.Xspeed = DEG2RAD(speed); limit = DEG2RAD(limit) + M.X; - }else{ + } + if(*G.axis == 'Y' || *G.axis == 'B'){ cmd.Ymot = DEG2RAD(target) + M.Y; cmd.Yspeed = DEG2RAD(speed); limit = DEG2RAD(limit) + M.Y; @@ -134,16 +132,21 @@ 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; - if(MCC_E_OK != Mount.init(&Config)){ + 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; } diff --git a/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c index f1d7ca0..73c50e4 100644 --- a/LibSidServo/examples/dumpswing.c +++ b/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; diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c index 0864adf..9ab7b18 100644 --- a/LibSidServo/examples/goto.c +++ b/LibSidServo/examples/goto.c @@ -23,6 +23,7 @@ #include #include +#include "conf.h" #include "dump.h" #include "sidservo.h" #include "simpleconv.h" @@ -33,6 +34,7 @@ typedef struct{ int wait; int relative; char *coordsoutput; + char *conffile; double X; double Y; } parameters; @@ -51,18 +53,10 @@ static sl_option_t cmdlnopts[] = { {"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_int, 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; @@ -86,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){ @@ -126,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/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c index 19ae664..924d634 100644 --- a/LibSidServo/examples/scmd_traectory.c +++ b/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_int, 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/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 6cedb0a..f63ec79 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files index 4adc54f..443fe64 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -1,5 +1,7 @@ CMakeLists.txt dbg.h +examples/conf.c +examples/conf.h examples/dump.c examples/dump.h examples/dumpmoving.c diff --git a/LibSidServo/main.c b/LibSidServo/main.c index a5fdf27..13b8476 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -104,12 +104,14 @@ static mcc_errcodes_t move2(const double *X, const double *Y){ if(!X && !Y) return MCC_E_BADFORMAT; if(X){ if(!chkX(*X)) return MCC_E_BADFORMAT; - int64_t tag = X_RAD2MOT(*X); + int32_t tag = X_RAD2MOT(*X); + DBG("X: %g, tag: %d", *X, tag); if(!SSsetterI(CMD_MOTX, tag)) return MCC_E_FAILED; } if(Y){ if(!chkY(*Y)) return MCC_E_BADFORMAT; - int64_t tag = X_RAD2MOT(*Y); + 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; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 344be02..24f8fb7 100644 --- a/LibSidServo/serial.c +++ b/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,7 +119,7 @@ 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 @@ -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){ diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 1f895a9..62e7d28 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -98,6 +98,10 @@ 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{ diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index 5f1594e..6a04241 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -137,7 +137,7 @@ int SSgetint(const char *cmd, int64_t *ans){ */ int SSsetterI(const char *cmd, int32_t ival){ char buf[128]; - snprintf(buf, 127, "%s%" PRIi64, cmd, ival); + snprintf(buf, 127, "%s%" PRIi32, cmd, ival); return SStextcmd(buf, NULL); } diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index 7911bf6..7445885 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -168,9 +168,7 @@ // 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 @@ -178,27 +176,27 @@ #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.)) // 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.)