diff --git a/LibSidServo/TODO b/LibSidServo/TODO deleted file mode 100644 index 9dae84f..0000000 --- a/LibSidServo/TODO +++ /dev/null @@ -1,2 +0,0 @@ -1. PID: slew2 - diff --git a/LibSidServo/examples/conf.c b/LibSidServo/examples/conf.c index 1271a21..5439925 100644 --- a/LibSidServo/examples/conf.c +++ b/LibSidServo/examples/conf.c @@ -52,7 +52,7 @@ static sl_option_t opts[] = { {"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", NO_ARGS, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"}, + {"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)"}, {"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"}, diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c index ed939eb..6bb6620 100644 --- a/LibSidServo/examples/dump.c +++ b/LibSidServo/examples/dump.c @@ -23,6 +23,9 @@ #include "dump.h" #include "simpleconv.h" +// starting dump time (to conform different logs) +static double dumpT0 = -1.; + #if 0 // amount of elements used for encoders' data filtering #define NFILT (10) @@ -59,6 +62,10 @@ static double filter(double val, int idx){ } #endif +// return starting time of dump +double dumpt0(){ return dumpT0; } + + /** * @brief logmnt - log mount data into file * @param fcoords - file to dump @@ -70,10 +77,10 @@ void logmnt(FILE *fcoords, mountdata_t *m){ if(!m){ // write header 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 fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n", - m->encXposition.t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val), + m->encXposition.t - dumpT0, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val), RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val), RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val), m->millis); @@ -139,6 +146,7 @@ void waitmoving(int N){ if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ xlast = mdata.motXposition.val; ylast = mdata.motYposition.val; + //DBG("x/y: %g/%g", RAD2DEG(xlast), RAD2DEG(ylast)); ctr = 0; }else ++ctr; } diff --git a/LibSidServo/examples/dump.h b/LibSidServo/examples/dump.h index 728dd27..0089539 100644 --- a/LibSidServo/examples/dump.h +++ b/LibSidServo/examples/dump.h @@ -27,3 +27,4 @@ void dumpmoving(FILE *fcoords, double t, int N); void waitmoving(int N); int getPos(coordval_pair_t *mot, coordval_pair_t *enc); void chk0(int ncycles); +double dumpt0(); diff --git a/LibSidServo/examples/dumpmoving_dragNtrack.c b/LibSidServo/examples/dumpmoving_dragNtrack.c index 67dcf71..487d826 100644 --- a/LibSidServo/examples/dumpmoving_dragNtrack.c +++ b/LibSidServo/examples/dumpmoving_dragNtrack.c @@ -216,7 +216,7 @@ int main(int argc, char **argv){ sleep(5); // return to zero and wait green("Return 2 zero and wait\n"); - if(!return2zero()) ERRX("Can't return"); + if(MCC_E_OK != return2zero()) ERRX("Can't return"); Wait(0., 0); Wait(0., 1); // wait moving ends diff --git a/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c index 2facc3d..01002ab 100644 --- a/LibSidServo/examples/dumpswing.c +++ b/LibSidServo/examples/dumpswing.c @@ -110,16 +110,28 @@ int main(int argc, char **argv){ return 1; } if(G.coordsoutput){ - if(!(fcoords = fopen(G.coordsoutput, "w"))) - ERRX("Can't open %s", G.coordsoutput); + if(!(fcoords = fopen(G.coordsoutput, "w"))){ + WARNX("Can't open %s", G.coordsoutput); + return 1; + } }else fcoords = stdout; - if(G.Ncycles < 7) ERRX("Ncycles should be >7"); + if(G.Ncycles < 2){ + WARNX("Ncycles should be >2"); + return 1; + } double absamp = fabs(G.amplitude); - if(absamp < 0.01 || absamp > 45.) - ERRX("Amplitude should be from 0.01 to 45 degrees"); - 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"); + if(absamp < 0.01 || absamp > 45.){ + WARNX("Amplitude should be from 0.01 to 45 degrees"); + return 1; + } + if(G.period < 0.1 || G.period > 900.){ + WARNX("Period should be from 0.1 to 900s"); + return 1; + } + if(G.Nswings < 1){ + WARNX("Nswings should be more than 0"); + return 1; + } conf_t *Config = readServoConf(G.conffile); if(!Config){ dumpConf(); diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c index d5a4b3e..15c0796 100644 --- a/LibSidServo/examples/goto.c +++ b/LibSidServo/examples/goto.c @@ -91,11 +91,10 @@ int main(int _U_ argc, char _U_ **argv){ if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount"); coordval_pair_t M, E; if(!getPos(&M, &E)) ERRX("Can't get current position"); + DBG("xt: %g, x: %g", M.X.t, M.X.val); if(G.coordsoutput){ - if(!G.wait) green("When logging I should wait until moving ends; added '-w'"); + if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n"); G.wait = 1; - } - if(G.coordsoutput){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); logmnt(fcoords, NULL); diff --git a/LibSidServo/examples/scmd_traectory.c b/LibSidServo/examples/scmd_traectory.c index 5643b5e..3d0e81c 100644 --- a/LibSidServo/examples/scmd_traectory.c +++ b/LibSidServo/examples/scmd_traectory.c @@ -98,7 +98,7 @@ static void runtraectory(traectory_fn tfn){ coordval_pair_t target; coordpair_t traectXY, endpoint; endpoint.X = G.Xmax, endpoint.Y = G.Ymax; - double t0 = Mount.currentT(), tlast = 0.; + double t0 = dumpt0(), tlast = 0., tstart = Mount.currentT(); double tlastX = 0., tlastY = 0.; while(1){ if(!telpos(&telXY)){ @@ -109,7 +109,7 @@ static void runtraectory(traectory_fn tfn){ DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f) measured @ %.6f/%.6f", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), telXY.X.t, telXY.Y.t); tlastX = telXY.X.t; tlastY = telXY.Y.t; double t = Mount.currentT(); - if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break; + if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break; if(!traectory_point(&traectXY, t)) break; target.X.val = traectXY.X; target.Y.val = traectXY.Y; target.X.t = target.Y.t = t; @@ -118,11 +118,14 @@ static void runtraectory(traectory_fn tfn){ 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; - //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); - DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); - //DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y)); - if(errlog) - fprintf(errlog, "%10.4g %10.4g %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + if(t0 < 0.) t0 = dumpt0(); + else{ + //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); + DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + //DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y)); + 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); tlast = t; diff --git a/LibSidServo/examples/servo.conf b/LibSidServo/examples/servo.conf index 220926e..9e7d3d4 100644 --- a/LibSidServo/examples/servo.conf +++ b/LibSidServo/examples/servo.conf @@ -1,4 +1,4 @@ -Current configuration: +# Current configuration MountDevPath=/dev/ttyUSB0 MountDevSpeed=19200 EncoderDevPath=(null) diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 73de00a..c62ce51 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -110,8 +110,8 @@ GenericProjectManager.GenericMakeStep 1 - Сборка - Сборка + Build + Build ProjectExplorer.BuildSteps.Build @@ -123,8 +123,8 @@ GenericProjectManager.GenericMakeStep 1 - Очистка - Очистка + Clean + Clean ProjectExplorer.BuildSteps.Clean 2 @@ -139,8 +139,8 @@ 0 - Развёртывание - Развёртывание + Deploy + Deploy ProjectExplorer.BuildSteps.Deploy 1 @@ -173,8 +173,8 @@ 0 - Развёртывание - Развёртывание + Deploy + Deploy ProjectExplorer.BuildSteps.Deploy 1 diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 52b9e7b..d6d267c 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -51,17 +51,18 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd); * @return time in seconds */ double nanotime(){ - static struct timespec *start = NULL; + //static struct timespec *start = NULL; struct timespec now; - if(!start){ + /*if(!start){ start = malloc(sizeof(struct timespec)); if(!start) return -1.; if(clock_gettime(CLOCK_MONOTONIC, start)) return -1.; - } + }*/ if(clock_gettime(CLOCK_MONOTONIC, &now)) return -1.; - double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9; + /*double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9; double sd = (double)now.tv_sec - (double)start->tv_sec; - return sd + nd; + return sd + nd;*/ + return (double)now.tv_sec + (double)now.tv_nsec * 1e-9; } /** @@ -75,7 +76,7 @@ static void quit(){ DBG("Exit"); } -void getModData(coordval_pair_t *c){ +void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst){ if(!c || !Xmodel || !Ymodel) return; double tnow = nanotime(); moveparam_t Xp, Yp; @@ -87,6 +88,8 @@ void getModData(coordval_pair_t *c){ c->X.t = c->Y.t = tnow; c->X.val = Xp.coord; c->Y.val = Yp.coord; + if(xst) *xst = Xst; + if(yst) *yst = Yst; } /** diff --git a/LibSidServo/main.h b/LibSidServo/main.h index f575547..c8ae4c9 100644 --- a/LibSidServo/main.h +++ b/LibSidServo/main.h @@ -24,11 +24,12 @@ #include +#include "movingmodel.h" #include "sidservo.h" extern conf_t Conf; double nanotime(); -void getModData(coordval_pair_t *c); +void getModData(coordval_pair_t *c, movestate_t *xst, movestate_t *yst); typedef struct{ double *x, *t, *t2, *xt; // arrays of coord/time and multiply double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply diff --git a/LibSidServo/movingmodel.c b/LibSidServo/movingmodel.c index a24439f..0472459 100644 --- a/LibSidServo/movingmodel.c +++ b/LibSidServo/movingmodel.c @@ -60,9 +60,14 @@ movemodel_t *model_init(limits_t *l){ int model_move2(movemodel_t *model, moveparam_t *target, double t){ if(!target || !model) return FALSE; - //DBG("MOVE to %g at speed %g", target->coord, target->speed); + DBG("MOVE to %g at speed %g", target->coord, target->speed); // only positive velocity if(target->speed < 0.) target->speed = -target->speed; + if(fabs(target->speed) < model->Min.speed){ + DBG("STOP"); + model->stop(model, t); + return TRUE; + } // don't mind about acceleration - user cannot set it now return model->calculate(model, target, t); } diff --git a/LibSidServo/ramp.c b/LibSidServo/ramp.c index c6edc74..b9f4728 100644 --- a/LibSidServo/ramp.c +++ b/LibSidServo/ramp.c @@ -23,12 +23,14 @@ #include "main.h" #include "ramp.h" -/* + #ifdef EBUG #undef DBG #define DBG(...) +#undef FNAME +#define FNAME() #endif -*/ + static double coord_tolerance = COORD_TOLERANCE_DEFAULT; static void emstop(movemodel_t *m, double _U_ t){ diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 5f426cf..5b5c410 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -59,13 +59,15 @@ typedef struct __attribute__((packed)){ // calculate current X/Y speeds void getXspeed(){ + static double t0 = -1.; // time of start - eliminate problem of very large times in squares + if(t0 < 0.) t0 = mountdata.encXposition.t; static less_square_t *ls = NULL; if(!ls){ ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); if(!ls) return; } pthread_mutex_lock(&datamutex); - double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t); + double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t - t0); if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){ mountdata.encXspeed.val = speed; mountdata.encXspeed.t = mountdata.encXposition.t; @@ -80,13 +82,15 @@ void getXspeed(){ #endif } void getYspeed(){ + static double t0 = -1.; // time of start - eliminate problem of very large times in squares + if(t0 < 0.) t0 = mountdata.encXposition.t; static less_square_t *ls = NULL; if(!ls){ ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval); if(!ls) return; } pthread_mutex_lock(&datamutex); - double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t); + double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t - t0); if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){ mountdata.encYspeed.val = speed; mountdata.encYspeed.t = mountdata.encYposition.t; @@ -392,21 +396,31 @@ static void *mountthread(void _U_ *u){ uint8_t buf[2*sizeof(SSstat)]; SSstat *status = (SSstat*) buf; bzero(&mountdata, sizeof(mountdata)); - double t0 = nanotime(); + double t0 = nanotime(), tstart = t0; static double oldmt = -100.; // old `millis measurement` time static uint32_t oldmillis = 0; if(Conf.RunModel) while(1){ coordval_pair_t c; + movestate_t xst, yst; // now change data - getModData(&c); + getModData(&c, &xst, &yst); pthread_mutex_lock(&datamutex); double tnow = c.X.t; - mountdata.motXposition.t = mountdata.encXposition.t = mountdata.motYposition.t = mountdata.encYposition.t = tnow; - mountdata.motXposition.val = mountdata.encXposition.val = c.X.val; - mountdata.motYposition.val = mountdata.encYposition.val = c.Y.val; + mountdata.encXposition.t = mountdata.encYposition.t = tnow; + mountdata.encXposition.val = c.X.val; + mountdata.encYposition.val = c.Y.val; //DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val); if(tnow - oldmt > Conf.MountReqInterval){ - oldmillis = mountdata.millis = (uint32_t)(tnow * 1e3); + oldmillis = mountdata.millis = (uint32_t)((tnow - tstart) * 1e3); + mountdata.motYposition.t = mountdata.motXposition.t = tnow; + if(xst == ST_MOVE) + mountdata.motXposition.val = c.X.val + (c.X.val - mountdata.motXposition.val)*(drand48() - 0.5)/100.; + else + mountdata.motXposition.val = c.X.val; + if(yst == ST_MOVE) + mountdata.motYposition.val = c.Y.val + (c.Y.val - mountdata.motYposition.val)*(drand48() - 0.5)/100.; + else + mountdata.motYposition.val = c.Y.val; oldmt = tnow; }else mountdata.millis = oldmillis; pthread_mutex_unlock(&datamutex);