diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b420d1..ef54794 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,8 +14,8 @@ set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) set(EXAMPLES OFF CACHE BOOL "" FORCE) # set(CMAKE_BUILD_TYPE "Release") -add_subdirectory(LibSidServo) set(CMAKE_BUILD_TYPE "Debug") +add_subdirectory(LibSidServo) # add_subdirectory(cxx) add_subdirectory(mcc) diff --git a/LibSidServo/PID.c b/LibSidServo/PID.c index 4a3e874..ae7076d 100644 --- a/LibSidServo/PID.c +++ b/LibSidServo/PID.c @@ -84,41 +84,42 @@ typedef struct{ * @return calculated new speed or -1 for max speed */ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){ - if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT){ - DBG("target time: %g, axis time: %g - too big! (%g)", tagpos->t, axis->position.t, MCC_PID_MAX_DT); + if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > Conf.PIDMaxDt){ + DBG("target time: %g, axis time: %g - too big! (tag-ax=%g)", tagpos->t, axis->position.t, tagpos->t - axis->position.t); return axis->speed.val; // data is too old or wrong } double error = tagpos->val - axis->position.val, fe = fabs(error); + DBG("error: %g", error); PIDController_t *pid = NULL; switch(axis->state){ case AXIS_SLEWING: - if(fe < MCC_MAX_POINTING_ERR){ + if(fe < Conf.MaxPointingErr){ axis->state = AXIS_POINTING; DBG("--> Pointing"); pid = pidpair->PIDC; }else{ DBG("Slewing..."); - return -1.; // max speed for given axis + return NAN; // max speed for given axis } break; case AXIS_POINTING: - if(fe < MCC_MAX_GUIDING_ERR){ + if(fe < Conf.MaxFinePoingintErr){ axis->state = AXIS_GUIDING; DBG("--> Guiding"); pid = pidpair->PIDV; - }else if(fe > MCC_MAX_POINTING_ERR){ + }else if(fe > Conf.MaxPointingErr){ DBG("--> Slewing"); axis->state = AXIS_SLEWING; - return -1.; + return NAN; } else pid = pidpair->PIDC; break; case AXIS_GUIDING: pid = pidpair->PIDV; - if(fe > MCC_MAX_GUIDING_ERR){ + if(fe > Conf.MaxFinePoingintErr){ DBG("--> Pointing"); axis->state = AXIS_POINTING; pid = pidpair->PIDC; - }else if(fe < MCC_MAX_ATTARGET_ERR){ + }else if(fe < Conf.MaxGuidingErr){ DBG("At target"); // TODO: we can point somehow that we are at target or introduce new axis state }else DBG("Current error: %g", fe); @@ -126,7 +127,7 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t case AXIS_STOPPED: // start pointing to target; will change speed next time DBG("AXIS STOPPED!!!!"); axis->state = AXIS_SLEWING; - return -1.; + return NAN; case AXIS_ERROR: DBG("Can't move from erroneous state"); return 0.; @@ -135,12 +136,12 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t DBG("WTF? Where is a PID?"); return axis->speed.val; } - if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){ + if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > Conf.PIDMaxDt){ DBG("time diff too big: clear PID"); pid_clear(pid); } double dt = tagpos->t - pid->prevT; - if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME; + if(dt > Conf.PIDMaxDt) dt = Conf.PIDCycleDt; pid->prevT = tagpos->t; DBG("CALC PID (er=%g, dt=%g), state=%d", error, dt, axis->state); double tagspeed = pid_calculate(pid, error, dt); @@ -154,22 +155,23 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t * @param endpoint - stop point (some far enough point to stop in case of hang) * @return error code */ -mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint){ +mcc_errcodes_t correct2(const coordval_pair_t *target){ static PIDpair_t pidX = {0}, pidY = {0}; if(!pidX.PIDC){ - pidX.PIDC = pid_create(&Conf.XPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); + pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); if(!pidX.PIDC) return MCC_E_FATAL; - pidX.PIDV = pid_create(&Conf.XPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); + pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); if(!pidX.PIDV) return MCC_E_FATAL; } if(!pidY.PIDC){ - pidY.PIDC = pid_create(&Conf.YPIDC, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); + pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); if(!pidY.PIDC) return MCC_E_FATAL; - pidY.PIDV = pid_create(&Conf.YPIDV, MCC_PID_CYCLE_TIME / MCC_PID_REFRESH_DT); + pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); if(!pidY.PIDV) return MCC_E_FATAL; } mountdata_t m; - coordpair_t tagspeed; + coordpair_t tagspeed; // absolute value of speed + double Xsign = 1., Ysign = 1.; // signs of speed (for target calculation) if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED; axisdata_t axis; DBG("state: %d/%d", m.Xstate, m.Ystate); @@ -177,20 +179,42 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi axis.position = m.encXposition; axis.speed = m.encXspeed; tagspeed.X = getspeed(&target->X, &pidX, &axis); - if(tagspeed.X < 0.) tagspeed.X = -tagspeed.X; - if(tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED; + if(isnan(tagspeed.X)){ // max speed + if(target->X.val < axis.position.val) Xsign = -1.; + tagspeed.X = Xlimits.max.speed; + }else{ + if(tagspeed.X < 0.){ tagspeed.X = -tagspeed.X; Xsign = -1.; } + if(tagspeed.X > Xlimits.max.speed) tagspeed.X = Xlimits.max.speed; + } axis_status_t xstate = axis.state; axis.state = m.Ystate; axis.position = m.encYposition; axis.speed = m.encYspeed; tagspeed.Y = getspeed(&target->Y, &pidY, &axis); - if(tagspeed.Y < 0.) tagspeed.Y = -tagspeed.Y; - if(tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED; + if(isnan(tagspeed.Y)){ // max speed + if(target->Y.val < axis.position.val) Ysign = -1.; + tagspeed.Y = Ylimits.max.speed; + }else{ + if(tagspeed.Y < 0.){ tagspeed.Y = -tagspeed.Y; Ysign = -1.; } + if(tagspeed.Y > Ylimits.max.speed) tagspeed.Y = Ylimits.max.speed; + } axis_status_t ystate = axis.state; if(m.Xstate != xstate || m.Ystate != ystate){ DBG("State changed"); setStat(xstate, ystate); } - DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y); - return Mount.moveWspeed(endpoint, &tagspeed); + coordpair_t endpoint; + // allow at least PIDMaxDt moving with target speed + double dv = fabs(tagspeed.X - m.encXspeed.val); + double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed + + Conf.PIDMaxDt * tagspeed.X // PIDMaxDt const speed moving + + tagspeed.X * tagspeed.X / Xlimits.max.accel / 2.; // stopping + endpoint.X = m.encXposition.val + Xsign * adder; + dv = fabs(tagspeed.Y - m.encYspeed.val); + adder = dv/Ylimits.max.accel * (m.encYspeed.val + dv / 2.) + + Conf.PIDMaxDt * tagspeed.Y + + tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.; + endpoint.Y = m.encYposition.val + Ysign * tagspeed.Y / Ylimits.max.accel; + DBG("TAG speeds: %g/%g; TAG pos: %g/%g", tagspeed.X, tagspeed.Y, endpoint.X, endpoint.Y); + return Mount.moveWspeed(&endpoint, &tagspeed); } diff --git a/LibSidServo/PID.h b/LibSidServo/PID.h index 5eb8cb9..30f1aa2 100644 --- a/LibSidServo/PID.h +++ b/LibSidServo/PID.h @@ -37,4 +37,4 @@ void pid_clear(PIDController_t *pid); void pid_delete(PIDController_t **pid); double pid_calculate(PIDController_t *pid, double error, double dt); -mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoint); +mcc_errcodes_t correct2(const coordval_pair_t *target); diff --git a/LibSidServo/dbg.h b/LibSidServo/dbg.h deleted file mode 100644 index 50b3401..0000000 --- a/LibSidServo/dbg.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - * 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 - -#include "sidservo.h" - -extern conf_t Conf; - -// unused arguments of functions -#define _U_ __attribute__((__unused__)) -// break absent in `case` -#define FALLTHRU __attribute__ ((fallthrough)) -// and synonym for FALLTHRU -#define NOBREAKHERE __attribute__ ((fallthrough)) -// weak functions -#define WEAK __attribute__ ((weak)) - -#ifndef DBL_EPSILON -#define DBL_EPSILON (2.2204460492503131e-16) -#endif - -#ifndef FALSE -#define FALSE (0) -#endif - -#ifndef TRUE -#define TRUE (1) -#endif - - -#ifdef EBUG -#include - #define COLOR_RED "\033[1;31;40m" - #define COLOR_GREEN "\033[1;32;40m" - #define COLOR_OLD "\033[0;0;0m" - #define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \ - fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0) - #define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \ - fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ - fprintf(stderr, "\n");} while(0) - -#else // EBUG - #define FNAME() - #define DBG(...) -#endif // EBUG diff --git a/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c index 5439925..fe6ac2c 100644 --- a/LibSidServo/examples/conf.c +++ b/LibSidServo/examples/conf.c @@ -24,25 +24,32 @@ static conf_t Config = { .MountDevPath = "/dev/ttyUSB0", .MountDevSpeed = 19200, - .EncoderXDevPath = "/dev/encoderX0", - .EncoderYDevPath = "/dev/encoderY0", + .EncoderXDevPath = "/dev/encoder_X0", + .EncoderYDevPath = "/dev/encoder_Y0", .EncoderDevSpeed = 153000, .MountReqInterval = 0.1, - .EncoderReqInterval = 0.05, + .EncoderReqInterval = 0.001, .SepEncoder = 2, - .EncoderSpeedInterval = 0.1, - .XPIDC.P = 0.8, + .EncoderSpeedInterval = 0.05, + .EncodersDisagreement = 1e-5, // 2'' + .PIDMaxDt = 1., + .PIDRefreshDt = 0.1, + .PIDCycleDt = 5., + .XPIDC.P = 0.5, .XPIDC.I = 0.1, - .XPIDC.D = 0.3, - .XPIDV.P = 1., - .XPIDV.I = 0.01, - .XPIDV.D = 0.2, - .YPIDC.P = 0.8, + .XPIDC.D = 0.2, + .XPIDV.P = 0.09, + .XPIDV.I = 0.0, + .XPIDV.D = 0.05, + .YPIDC.P = 0.5, .YPIDC.I = 0.1, - .YPIDC.D = 0.3, - .YPIDV.P = 0.5, - .YPIDV.I = 0.2, - .YPIDV.D = 0.5, + .YPIDC.D = 0.2, + .YPIDV.P = 0.09, + .YPIDV.I = 0.0, + .YPIDV.D = 0.05, + .MaxPointingErr = 0.13962634, + .MaxFinePoingintErr = 0.026179939, + .MaxGuidingErr = 4.8481368e-7, }; static sl_option_t opts[] = { @@ -50,13 +57,17 @@ static sl_option_t opts[] = { {"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)"}, - {"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"}, {"SepEncoder", NEED_ARG, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"}, {"EncoderXDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderXDevPath), "path to X encoder (/dev/encoderX0)"}, {"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"}, + {"EncodersDisagreement", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncodersDisagreement),"acceptable disagreement between motor and axis encoders"}, + {"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"}, + {"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"}, {"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"}, {"RunModel", NEED_ARG, NULL, 0, arg_int, APTR(&Config.RunModel), "instead of real hardware run emulation"}, + {"PIDMaxDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDMaxDt), "maximal PID refresh time interval (if larger all old data will be cleared)"}, + {"PIDRefreshDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDRefreshDt), "normal PID refresh interval by master process"}, + {"PIDCycleDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDCycleDt), "PID I cycle time (analog of \"RC\" for PID on opamps)"}, {"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"}, {"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"}, {"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"}, @@ -69,6 +80,10 @@ static sl_option_t opts[] = { {"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"}, {"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"}, {"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"}, + {"MaxPointingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxPointingErr), "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"}, + {"MaxFinePoingintErr",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxFinePoingintErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"}, + {"MaxGuidingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxGuidingErr), "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"}, + // {"",NEED_ARG, NULL, 0, arg_double, APTR(&Config.), ""}, end_option }; @@ -93,5 +108,5 @@ void dumpConf(){ } void confHelp(){ - sl_showhelp(-1, opts); + sl_conf_showhelp(-1, opts); } diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c index 70e78c4..a04b375 100644 --- a/LibSidServo/examples/dump.c +++ b/LibSidServo/examples/dump.c @@ -75,7 +75,7 @@ void logmnt(FILE *fcoords, mountdata_t *m){ if(!fcoords) return; //DBG("LOG %s", m ? "data" : "header"); if(!m){ // write header - fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n"); + fprintf(fcoords, " time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n"); return; }else if(dumpT0 < 0.) dumpT0 = m->encXposition.t; // write data diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index f979590..9b0c111 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/LibSidServo/examples/dumpmoving.c @@ -73,6 +73,7 @@ int main(int argc, char **argv){ conf_t *Config = readServoConf(G.conffile); if(!Config){ dumpConf(); + confHelp(); return 1; } if(G.coordsoutput){ diff --git a/LibSidServo/examples/dumpmoving_dragNtrack.c b/LibSidServo/examples/dumpmoving_dragNtrack.c index 487d826..07d5b71 100644 --- a/LibSidServo/examples/dumpmoving_dragNtrack.c +++ b/LibSidServo/examples/dumpmoving_dragNtrack.c @@ -139,8 +139,10 @@ static mcc_errcodes_t return2zero(){ short_command_t cmd = {0}; DBG("Try to move to zero"); cmd.Xmot = 0.; cmd.Ymot = 0.; - cmd.Xspeed = MCC_MAX_X_SPEED; - cmd.Yspeed = MCC_MAX_Y_SPEED; + coordpair_t maxspd; + if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED; + cmd.Xspeed = maxspd.X; + cmd.Yspeed = maxspd.Y; /*cmd.xychange = 1; cmd.XBits = 100; cmd.YBits = 20;*/ diff --git a/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c index 3d0e81c..53cbce6 100644 --- a/LibSidServo/examples/scmd_traectory.c +++ b/LibSidServo/examples/scmd_traectory.c @@ -44,6 +44,7 @@ typedef struct{ char *conffile; } parameters; +static conf_t *Config = NULL; static FILE *fcoords = NULL, *errlog = NULL; static pthread_t dthr; static parameters G = { @@ -96,8 +97,7 @@ static void runtraectory(traectory_fn tfn){ if(!tfn) return; coordval_pair_t telXY; coordval_pair_t target; - coordpair_t traectXY, endpoint; - endpoint.X = G.Xmax, endpoint.Y = G.Ymax; + coordpair_t traectXY; double t0 = dumpt0(), tlast = 0., tstart = Mount.currentT(); double tlastX = 0., tlastY = 0.; while(1){ @@ -113,11 +113,6 @@ static void runtraectory(traectory_fn tfn){ if(!traectory_point(&traectXY, t)) break; target.X.val = traectXY.X; target.Y.val = traectXY.Y; target.X.t = target.Y.t = t; - // check whether we should change direction - if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax; - else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax; - if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax; - else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax; if(t0 < 0.) t0 = dumpt0(); else{ //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); @@ -126,8 +121,8 @@ static void runtraectory(traectory_fn tfn){ if(errlog) fprintf(errlog, "%10.4f %10.4f %10.4f\n", telXY.X.t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); } - if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!"); - while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50); + if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!"); + while((t = Mount.currentT()) - tlast < Config->PIDRefreshDt) usleep(500); tlast = t; } WARNX("No next traectory point or emulation ends"); @@ -153,7 +148,7 @@ int main(int argc, char **argv){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); }else fcoords = stdout; - conf_t *Config = readServoConf(G.conffile); + Config = readServoConf(G.conffile); if(!Config || G.dumpconf){ dumpConf(); return 1; diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index c62ce51..dfa9ebb 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -86,6 +86,7 @@ true + 0 @@ -164,6 +165,7 @@ ProjectExplorer.CustomExecutableRunConfiguration false + true true @@ -198,6 +200,7 @@ ProjectExplorer.CustomExecutableRunConfiguration false + true true @@ -208,10 +211,6 @@ ProjectExplorer.Project.TargetCount 1 - - ProjectExplorer.Project.Updater.FileVersion - 22 - Version 22 diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 5fe1b9d..b4b44de 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -35,14 +35,17 @@ conf_t Conf = {0}; // parameters for model static movemodel_t *Xmodel, *Ymodel; +// limits for model and/or real mount (in latter case data should be read from mount on init) // radians, rad/sec, rad/sec^2 -static limits_t +// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s +// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2 +limits_t Xlimits = { .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, - .max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}}, + .max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}}, Ylimits = { .min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6}, - .max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}} + .max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}} ; static mcc_errcodes_t shortcmd(short_command_t *cmd); @@ -58,7 +61,7 @@ double nanotime(){ struct timespec start; if(clock_gettime(CLOCK_REALTIME, &start)) return -1.; t0 = (double)start.tv_sec + (double)start.tv_nsec * 1e-9 - - (double)now.tv_sec + (double)now.tv_nsec * 1e-9; + - (double)now.tv_sec - (double)now.tv_nsec * 1e-9; } return (double)now.tv_sec + (double)now.tv_nsec * 1e-9 + t0; } @@ -136,7 +139,6 @@ double LS_calc_slope(less_square_t *l, double x, double t){ return (numerator / denominator); } - /** * @brief init - open serial devices and do other job * @param c - initial configuration @@ -149,11 +151,15 @@ static mcc_errcodes_t init(conf_t *c){ mcc_errcodes_t ret = MCC_E_OK; Xmodel = model_init(&Xlimits); Ymodel = model_init(&Ylimits); + if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){ + DBG("Bad value of MountReqInterval"); + ret = MCC_E_BADFORMAT; + } if(Conf.RunModel){ if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED; return MCC_E_OK; } - if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){ + if(!Conf.MountDevPath || Conf.MountDevSpeed < MOUNT_BAUDRATE_MIN){ DBG("Define mount device path and speed"); ret = MCC_E_BADFORMAT; }else if(!openMount()){ @@ -169,16 +175,11 @@ static mcc_errcodes_t init(conf_t *c){ ret = MCC_E_ENCODERDEV; } } - if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){ - DBG("Bad value of MountReqInterval"); - ret = MCC_E_BADFORMAT; - } + // TODO: read hardware configuration on init if(Conf.EncoderSpeedInterval < Conf.EncoderReqInterval * MCC_CONF_MIN_SPEEDC || Conf.EncoderSpeedInterval > MCC_CONF_MAX_SPEEDINT){ DBG("Wrong speed interval"); ret = MCC_E_BADFORMAT; } - //uint8_t buf[1024]; - //data_t d = {.buf = buf, .len = 0, .maxlen = 1024}; if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED; if(ret != MCC_E_OK) return ret; return updateMotorPos(); @@ -187,19 +188,19 @@ static mcc_errcodes_t init(conf_t *c){ // check coordinates (rad) and speeds (rad/s); 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; + if(X > Xlimits.max.coord || X < Xlimits.min.coord) return FALSE; return TRUE; } static int chkY(double Y){ - if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE; + if(Y > Ylimits.max.coord || Y < Ylimits.min.coord) return FALSE; return TRUE; } static int chkXs(double s){ - if(s < 0. || s > MCC_MAX_X_SPEED) return FALSE; + if(s < Xlimits.min.speed || s > Xlimits.max.speed) return FALSE; return TRUE; } static int chkYs(double s){ - if(s < 0. || s > MCC_MAX_Y_SPEED) return FALSE; + if(s < Ylimits.min.speed || s > Ylimits.max.speed) return FALSE; return TRUE; } @@ -246,8 +247,8 @@ static mcc_errcodes_t move2(const coordpair_t *target){ DBG("x,y: %g, %g", target->X, target->Y); cmd.Xmot = target->X; cmd.Ymot = target->Y; - cmd.Xspeed = MCC_MAX_X_SPEED; - cmd.Yspeed = MCC_MAX_Y_SPEED; + cmd.Xspeed = Xlimits.max.speed; + cmd.Yspeed = Ylimits.max.speed; mcc_errcodes_t r = shortcmd(&cmd); if(r != MCC_E_OK) return r; setslewingstate(); @@ -280,6 +281,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed if(!target || !speed) return MCC_E_BADFORMAT; if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT; + // updateMotorPos() here can make a problem; TODO: remove? if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED; short_command_t cmd = {0}; cmd.Xmot = target->X; @@ -498,13 +500,33 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){ config.backlspd = X_RS2MOTSPD(hwConfig->backlspd); config.Xsetpr = __bswap_32(hwConfig->Xsetpr); config.Ysetpr = __bswap_32(hwConfig->Ysetpr); - config.Xmetpr = __bswap_32(hwConfig->Xmetpr); - config.Ymetpr = __bswap_32(hwConfig->Ymetpr); + config.Xmetpr = __bswap_32(hwConfig->Xmetpr * 4); + config.Ymetpr = __bswap_32(hwConfig->Ymetpr * 4); // TODO - next (void) config; return MCC_E_OK; } +// getters of max/min speed and acceleration +mcc_errcodes_t maxspeed(coordpair_t *v){ + if(!v) return MCC_E_BADFORMAT; + v->X = Xlimits.max.speed; + v->Y = Ylimits.max.speed; + return MCC_E_OK; +} +mcc_errcodes_t minspeed(coordpair_t *v){ + if(!v) return MCC_E_BADFORMAT; + v->X = Xlimits.min.speed; + v->Y = Ylimits.min.speed; + return MCC_E_OK; +} +mcc_errcodes_t acceleration(coordpair_t *a){ + if(!a) return MCC_E_BADFORMAT; + a->X = Xlimits.max.accel; + a->Y = Ylimits.max.accel; + return MCC_E_OK; +} + // init mount class mount_t Mount = { .init = init, @@ -522,5 +544,8 @@ mount_t Mount = { .saveHWconfig = write_hwconf, .currentT = nanotime, .correctTo = correct2, + .getMaxSpeed = maxspeed, + .getMinSpeed = minspeed, + .getAcceleration = acceleration, }; diff --git a/LibSidServo/main.h b/LibSidServo/main.h index c8ae4c9..4246b1c 100644 --- a/LibSidServo/main.h +++ b/LibSidServo/main.h @@ -28,6 +28,7 @@ #include "sidservo.h" extern conf_t Conf; +extern limits_t Xlimits, Ylimits; double nanotime(); void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst); typedef struct{ @@ -43,10 +44,6 @@ double LS_calc_slope(less_square_t *l, double x, double t); // unused arguments of functions #define _U_ __attribute__((__unused__)) -// break absent in `case` -#define FALLTHRU __attribute__ ((fallthrough)) -// and synonym for FALLTHRU -#define NOBREAKHERE __attribute__ ((fallthrough)) // weak functions #define WEAK __attribute__ ((weak)) diff --git a/LibSidServo/movingmodel.h b/LibSidServo/movingmodel.h index d7b75bb..dfd7ab9 100644 --- a/LibSidServo/movingmodel.h +++ b/LibSidServo/movingmodel.h @@ -44,7 +44,7 @@ typedef struct{ typedef struct{ moveparam_t min; moveparam_t max; - double acceleration; + //double acceleration; } limits_t; typedef enum{ diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index a9bfc77..e550945 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -68,7 +68,7 @@ void getXspeed(){ } pthread_mutex_lock(&datamutex); double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t - t0); - if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){ + if(fabs(speed) < 1.5 * Xlimits.max.speed){ mountdata.encXspeed.val = speed; mountdata.encXspeed.t = mountdata.encXposition.t; } @@ -91,7 +91,7 @@ void getYspeed(){ } pthread_mutex_lock(&datamutex); double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t - t0); - if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){ + if(fabs(speed) < 1.5 * Ylimits.max.speed){ mountdata.encYspeed.val = speed; mountdata.encYspeed.t = mountdata.encYposition.t; } @@ -149,10 +149,10 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){ DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val); mountdata.encXposition.t = t; mountdata.encYposition.t = t; + pthread_mutex_unlock(&datamutex); //if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(); //if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(); getXspeed(); getYspeed(); - 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); } @@ -790,13 +790,14 @@ int cmdC(SSconfig *conf, int rw){ 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; + if(d.len != d.maxlen){ ret = FALSE; goto rtn; } // 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; + ret = FALSE; + goto rtn; } } rtn: diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index a3dc44c..ae46249 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -32,38 +32,13 @@ extern "C" #include #include -// acceptable position error - 0.1'' -#define MCC_POSITION_ERROR (5e-7) -// acceptable disagreement between motor and axis encoders - 2'' -#define MCC_ENCODERS_ERROR (1e-7) - -// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s -#define MCC_MAX_X_SPEED (0.174533) -#define MCC_MAX_Y_SPEED (0.139626) -// accelerations by both axis (for model); TODO: move speeds/accelerations into config? -// xa=12.6 deg/s^2, ya= 9.5 deg/s^2 -#define MCC_X_ACCELERATION (0.219911) -#define MCC_Y_ACCELERATION (0.165806) - +// minimal serial speed of mount device +#define MOUNT_BAUDRATE_MIN (1200) // max speed interval, seconds #define MCC_CONF_MAX_SPEEDINT (2.) // minimal speed interval in parts of EncoderReqInterval #define MCC_CONF_MIN_SPEEDC (3.) -// PID I cycle time (analog of "RC" for PID on opamps) -#define MCC_PID_CYCLE_TIME (5.) -// maximal PID refresh time interval (if larger all old data will be cleared) -#define MCC_PID_MAX_DT (1.) -// normal PID refresh interval -#define MCC_PID_REFRESH_DT (0.1) -// boundary conditions for axis state: "slewing/pointing/guiding" -// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 8 degrees -//#define MCC_MAX_POINTING_ERR (0.20943951) -//#define MCC_MAX_POINTING_ERR (0.08726646) -#define MCC_MAX_POINTING_ERR (0.13962634) -// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg -#define MCC_MAX_GUIDING_ERR (0.026179939) -// if error less than this value we suppose that target is captured and guiding is good: 0.1'' -#define MCC_MAX_ATTARGET_ERR (4.8481368e-7) + // error codes typedef enum{ @@ -87,14 +62,21 @@ typedef struct{ int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices char* EncoderXDevPath; // paths to new controller devices char* EncoderYDevPath; + double EncodersDisagreement; // acceptable disagreement between motor and axis encoders double MountReqInterval; // interval between subsequent mount requests (seconds) double EncoderReqInterval; // interval between subsequent encoder requests (seconds) double EncoderSpeedInterval; // interval between speed calculations int RunModel; // == 1 if you want to use model instead of real mount + double PIDMaxDt; // maximal PID refresh time interval (if larger all old data will be cleared) + double PIDRefreshDt; // normal PID refresh interval + double PIDCycleDt; // PID I cycle time (analog of "RC" for PID on opamps) PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven) PIDpar_t XPIDV; PIDpar_t YPIDC; PIDpar_t YPIDV; + double MaxPointingErr; // if angle < this, change state from "slewing" to "pointing" (coarse pointing): 8 degrees + double MaxFinePoingintErr; // if angle < this, chane state from "pointing" to "guiding" (fine poinging): 1.5 deg + double MaxGuidingErr; // if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1'' } conf_t; // coordinates/speeds in degrees or d/s: X, Y @@ -247,7 +229,7 @@ typedef struct{ void (*quit)(); // deinit mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data // mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags); - mcc_errcodes_t (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint); + mcc_errcodes_t (*correctTo)(const coordval_pair_t *target); mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed @@ -258,6 +240,9 @@ typedef struct{ mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration mcc_errcodes_t (*saveHWconfig)(hardware_configuration_t *c); // save hardware configuration double (*currentT)(); // current time + mcc_errcodes_t (*getMaxSpeed)(coordpair_t *v); // maximal speed by both axis + mcc_errcodes_t (*getMinSpeed)(coordpair_t *v); // minimal -//- + mcc_errcodes_t (*getAcceleration)(coordpair_t *a); // acceleration/deceleration } mount_t; extern mount_t Mount; diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index cb4526a..6f5fd9c 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -183,21 +183,21 @@ mcc_errcodes_t updateMotorPos(){ if(MCC_E_OK == getMD(&md)){ if(md.encXposition.t == 0 || md.encYposition.t == 0){ DBG("Just started, t-t0 = %g!", t - t0); - sleep(1); DBG("t-t0 = %g", nanotime() - t0); //usleep(10000); continue; } + if(md.Xstate != AXIS_STOPPED || md.Ystate != AXIS_STOPPED) return MCC_E_OK; DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t); mcc_errcodes_t OK = MCC_E_OK; - if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ + if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){ DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val); if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){ DBG("Xpos sync failed!"); OK = MCC_E_FAILED; }else DBG("Xpos sync OK, Dt=%g", nanotime() - t0); } - if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ + if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){ DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val); if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){ DBG("Ypos sync failed!");