diff --git a/LibSidServo/movingfilter.c b/LibSidServo/movingfilter.c new file mode 100644 index 0000000..16001b2 --- /dev/null +++ b/LibSidServo/movingfilter.c @@ -0,0 +1,46 @@ +#include +#include +#include + +#define NFILT (5) + +static double filterK[NFILT]; + +static void buildFilter(){ + filterK[NFILT-1] = 1.; + double sum = 1.; + for(int i = NFILT-2; i > -1; --i){ + filterK[i] = (filterK[i+1] + 1.) * 1.1; + sum += filterK[i]; + } + for(int i = 0; i < NFILT; ++i){ + filterK[i] /= sum; + fprintf(stderr, "%d: %g\n", i, filterK[i]); + } +} + +static double filter(double val){ + static int ctr = 0; + static double lastvals[NFILT] = {0.}; + for(int i = NFILT-1; i > 0; --i) lastvals[i] = lastvals[i-1]; + lastvals[0] = val; + double r = 0.; + if(ctr < NFILT){ + ++ctr; + return val; + } + for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[i]; + return r; +} + +int main(int argc, char **argv){ + buildFilter(); + printf("Signal\tNoiced\tFiltered\n"); + for(int i = 0; i < 100; ++i){ + double di = (double)i; + double sig = di * di / 1e5 + sin(i * M_PI / 1500.); + double noiced = sig + 0.1 * (drand48() - 0.5); + printf("%.3f\t%.3f\t%.3f\n", sig, noiced, filter(noiced)); + } + return 0; +} \ No newline at end of file diff --git a/moving_model/Tramp.c b/moving_model/Tramp.c index 7711da8..a86835b 100644 --- a/moving_model/Tramp.c +++ b/moving_model/Tramp.c @@ -166,7 +166,7 @@ static int calcfromgs(moveparam_t *x, double t){ // calculations from non-stopped state static int calcfrommove(moveparam_t *x, double t){ double sign = (x->coord > curparams.coord) ? 1. : -1.; // signum of target accelerations and speeds - double curspdsign = (curparams.speed > 0.) ? 1. : -1.; + double curspeedsign = (curparams.speed > 0.) ? 1. : -1.; double absspeed = curparams.speed * sign; // abs speed value double dt = absspeed / Max.accel; // time to accelerate to current speed // check if target isn't too close for move in stopped mode @@ -179,7 +179,7 @@ static int calcfrommove(moveparam_t *x, double t){ return FALSE; // can't immediatelly stop } if(x->speed < absspeed) return calcfromgs(x, t); - if(sign * curspdsign > 0.){ // move into same side we are moving + if(sign * curspeedsign > 0.){ // move into same side we are moving return calcfromstop(x, t-dt, curparams.coord - xacc*sign); // just think that we are moving from past }else{ // move into opposite side: here we can't use trick with "moving from past" return calcfromopp(x, t); @@ -196,8 +196,68 @@ static int calc(moveparam_t *x, double t){ if(!x) return FALSE; if(x->coord < Min.coord || x->coord > Max.coord) return FALSE; if(x->speed < Min.speed || x->speed > Max.speed) return FALSE; - if(state == ST_STOP) return calcfromstop(x, t, curparams.coord); - else return calcfrommove(x, t); + double Dx = fabs(x->coord - curparams.coord); // full distance + double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds + // we have two variants: with or without stage with constant speed + double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed + double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs) + DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23); + double setspeed = x->speed; // new max speed (we can change it if need) + double dt01, dx01; // we'll fill them depending on starting conditions + Times[0] = t; + Params[0].speed = curparams.speed; + Params[0].coord = curparams.coord; + + double curspeed = fabs(curparams.speed); + double dt0s = curspeed / Max.accel; // time of stopping phase + double dx0s = curspeed * dt0s / 2.; // distance + DBG("dt0s=%g, dx0s=%g", dt0s, dx0s); + if(dx0s > Dx){ + DBG("distance too short"); + return FALSE; + } + if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target + DBG("Distance good to just stop"); + stop(t); + return TRUE; + } + if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign + // after stop we will have full profile + double dxs3 = Dx - dx0s; + double newspeed = sqrt(Max.accel * dxs3); + if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed + DBG("dxs3=%g, setspeed=%g", dxs3, setspeed); + dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; + Params[0].accel = sign * Max.accel; + dx01 = dx0s + setspeed / Max.accel / 2.; + }else{ // increase or decrease speed without stopping phase + dt01 = fabs(sign*setspeed - curparams.speed); + double a = (curspeed > setspeed) ? -Max.accel : Max.accel; + dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.; + DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01); + ;; + } + if(setspeed < Min.speed){ + DBG("New speed should be too small"); + return FALSE; + } + moveparam_t *p = &Params[STAGE_MAXSPEED]; + p->accel = 0.; p->speed = setspeed; + p->coord = curparams.coord + dx01 * sign; + dt23 = setspeed / Max.accel; + dx23 = setspeed * dt23 / 2.; + // calculate dx12 and dt12 + double dx12 = Dx - dx01 - dx23; + if(dx12 < 0.){ + DBG("Oops, WTF?"); + return FALSE; + } + double dt12 = dx12 / setspeed; + p = &Params[STAGE_DECEL]; + p->accel = -sign * Max.accel; + p->speed = setspeed; + p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12; + ;; } static movestate_t proc(moveparam_t *next, double t){ diff --git a/moving_model/main.c b/moving_model/main.c index 0e175a7..e033cf0 100644 --- a/moving_model/main.c +++ b/moving_model/main.c @@ -56,10 +56,6 @@ static int move(moveparam_t *tag){ if(!tag) ERRX("move(): needs target"); moveparam_t curpos; movestate_t curstate = model->get_state(&curpos); - if(curstate == ST_ERROR){ - WARNX("move(): got error state, can't move"); - return FALSE; - } if(!move_to(tag)){ WARNX("move(): can't move to %g with max speed %g", tag->coord, tag->speed); return FALSE; @@ -80,7 +76,7 @@ static void monit(double tnext){ movestate_t st = model->get_state(&p); fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\n", t - Tstart, p.coord, p.speed, p.accel); - if(st == ST_STOP || st == ST_ERROR) break; + if(st == ST_STOP) break; usleep(G.dT); }while(nanot() - t0 < tnext); DBG("End of monitoring"); diff --git a/moving_model/moving_model.config b/moving_model/moving_model.config index e0284f4..cadc51b 100644 --- a/moving_model/moving_model.config +++ b/moving_model/moving_model.config @@ -1,2 +1,4 @@ // Add predefined macros for your project here. For example: // #define THE_ANSWER 42 +#define _XOPEN_SOURCE 666 +#define EBUG diff --git a/moving_model/moving_model.creator.user b/moving_model/moving_model.creator.user index ac56112..765e0ab 100644 --- a/moving_model/moving_model.creator.user +++ b/moving_model/moving_model.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId