start changing T-ramp code (old was too ugly)

LibSidServo/movingfilter.c

@@ -0,0 +1,46 @@
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#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;
+}

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){

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");

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

moving_model/moving_model.creator.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 15.0.1, 2025-04-11T22:14:27. -->
+<!-- Written by QtCreator 15.0.1, 2025-04-13T17:59:08. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>