From e2c465dd30b85b8105a1e5f8580c25da24233424 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Mon, 19 May 2025 09:23:22 +0300 Subject: [PATCH] last version --- .../LibSidServo/examples/servo.conf | 8 + .../LibSidServo/moving_model/Dramp.c | 141 ++++++++++++ .../LibSidServo/moving_model/Dramp.h | 23 ++ .../LibSidServo/moving_model/Makefile | 57 +++++ .../LibSidServo/moving_model/Sramp.c | 27 +++ .../LibSidServo/moving_model/Sramp.h | 23 ++ .../LibSidServo/moving_model/Tramp.c | 217 ++++++++++++++++++ .../LibSidServo/moving_model/Tramp.h | 23 ++ .../LibSidServo/moving_model/main.c | 124 ++++++++++ .../LibSidServo/moving_model/moving.c | 125 ++++++++++ .../LibSidServo/moving_model/moving.h | 70 ++++++ .../moving_model/moving_model.cflags | 1 + .../moving_model/moving_model.config | 4 + .../moving_model/moving_model.creator | 1 + .../moving_model/moving_model.creator.user | 185 +++++++++++++++ .../moving_model.creator.user.7bd84e3 | 184 +++++++++++++++ .../moving_model/moving_model.cxxflags | 1 + .../moving_model/moving_model.files | 10 + .../moving_model/moving_model.includes | 0 .../LibSidServo/moving_model/moving_private.h | 26 +++ Auxiliary_utils/LibSidServo/moving_model/plot | 6 + .../LibSidServo/moving_model/plot_pdf | 5 + Auxiliary_utils/LibSidServo/movingfilter.c | 46 ++++ 23 files changed, 1307 insertions(+) create mode 100644 Auxiliary_utils/LibSidServo/examples/servo.conf create mode 100644 Auxiliary_utils/LibSidServo/moving_model/Dramp.c create mode 100644 Auxiliary_utils/LibSidServo/moving_model/Dramp.h create mode 100644 Auxiliary_utils/LibSidServo/moving_model/Makefile create mode 100644 Auxiliary_utils/LibSidServo/moving_model/Sramp.c create mode 100644 Auxiliary_utils/LibSidServo/moving_model/Sramp.h create mode 100644 Auxiliary_utils/LibSidServo/moving_model/Tramp.c create mode 100644 Auxiliary_utils/LibSidServo/moving_model/Tramp.h create mode 100644 Auxiliary_utils/LibSidServo/moving_model/main.c create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving.c create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving.h create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving_model.cflags create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving_model.config create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving_model.creator create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving_model.creator.user create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving_model.creator.user.7bd84e3 create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving_model.cxxflags create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving_model.files create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving_model.includes create mode 100644 Auxiliary_utils/LibSidServo/moving_model/moving_private.h create mode 100755 Auxiliary_utils/LibSidServo/moving_model/plot create mode 100755 Auxiliary_utils/LibSidServo/moving_model/plot_pdf create mode 100644 Auxiliary_utils/LibSidServo/movingfilter.c diff --git a/Auxiliary_utils/LibSidServo/examples/servo.conf b/Auxiliary_utils/LibSidServo/examples/servo.conf new file mode 100644 index 0000000..d37e959 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/examples/servo.conf @@ -0,0 +1,8 @@ +MountDevPath=/dev/ttyUSB0 +MountDevSpeed=19200 +EncoderXDevPath=/dev/encoder_X0 +EncoderYDevPath=/dev/encoder_Y0 +MountReqInterval=0.05 +SepEncoder=2 +EncoderReqInterval=0.01 +EncoderDevSpeed=1000000 diff --git a/Auxiliary_utils/LibSidServo/moving_model/Dramp.c b/Auxiliary_utils/LibSidServo/moving_model/Dramp.c new file mode 100644 index 0000000..df64442 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/Dramp.c @@ -0,0 +1,141 @@ +/* + * This file is part of the moving_model 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 + +#include "Dramp.h" + +static movestate_t state = ST_STOP; +static moveparam_t target, Min, Max; +static double T0 = -1., Xlast0; // time when move starts, last stage starting coordinate +static moveparam_t curparams = {0}; // current coordinate/speed/acceleration +static double T1 = -1.; // time to switch into minimal speed for best coord tolerance + +typedef enum{ + STAGE_NORMALSPEED, + STAGE_MINSPEED, + STAGE_STOPPED +} movingsage_t; + +static movingsage_t movingstage = STAGE_STOPPED; + +static int initlims(limits_t *lim){ + if(!lim) return FALSE; + Min = lim->min; + Max = lim->max; + return TRUE; +} + +static int calc(moveparam_t *x, double t){ + DBG("target: %g, tagspeed: %g (maxspeed: %g, minspeed: %g)", x->coord, x->speed, Max.speed, Min.speed); + if(!x || t < 0.) return FALSE; + if(x->speed > Max.speed || x->speed < Min.speed || x->coord < Min.coord || x->coord > Max.coord) return FALSE; + double adist = fabs(x->coord - curparams.coord); + DBG("want dist: %g", adist); + if(adist < coord_tolerance) return TRUE; // we are at place + if(adist < time_tick * Min.speed) return FALSE; // cannot reach with current parameters + target = *x; + if(x->speed * time_tick > adist) target.speed = adist / (10. * time_tick); // take at least 10 ticks to reach position + if(target.speed < Min.speed) target.speed = Min.speed; + DBG("Approximate tag speed: %g", target.speed); + T0 = t; + // calculate time to switch into minimal speed + T1 = -1.; // no min speed phase + if(target.speed > Min.speed){ + double dxpertick = target.speed * time_tick; + DBG("dX per one tick: %g", dxpertick); + double ticks_need = floor(adist / dxpertick); + DBG("ticks need: %g", ticks_need); + if(ticks_need < 1.) return FALSE; // cannot reach + if(fabs(ticks_need * dxpertick - adist) > coord_tolerance){ + DBG("Need to calculate slow phase; can't reach for %g ticks at current speed", ticks_need); + double dxpersmtick = Min.speed * time_tick; + DBG("dX per smallest tick: %g", dxpersmtick); + while(--ticks_need > 1.){ + double part = adist - ticks_need * dxpertick; + double smticks = floor(part / dxpersmtick); + double least = part - smticks * dxpersmtick; + if(least < coord_tolerance) break; + } + DBG("now BIG ticks: %g, T1=T0+%g", ticks_need, ticks_need*time_tick); + T1 = t + ticks_need * time_tick; + } + } + state = ST_MOVE; + Xlast0 = curparams.coord; + if(target.speed > Min.speed) movingstage = STAGE_NORMALSPEED; + else movingstage = STAGE_MINSPEED; + if(x->coord < curparams.coord) target.speed *= -1.; // real speed + curparams.speed = target.speed; + return TRUE; +} + +static void stop(double _U_ t){ + T0 = -1.; + curparams.accel = 0.; + curparams.speed = 0.; + state = ST_STOP; + movingstage = STAGE_STOPPED; +} + +static movestate_t proc(moveparam_t *next, double t){ + if(T0 < 0.) return ST_STOP; + curparams.coord = Xlast0 + (t - T0) * curparams.speed; + //DBG("coord: %g (dT: %g, speed: %g)", curparams.coord, t-T0, curparams.speed); + int ooops = FALSE; // oops - we are over target! + if(curparams.speed < 0.){ if(curparams.coord < target.coord) ooops = TRUE;} + else{ if(curparams.coord > target.coord) ooops = TRUE; } + if(ooops){ + DBG("OOOps! We are (%g) over target (%g) -> stop", curparams.coord, target.coord); + stop(t); + if(next) *next = curparams; + return state; + } + if(movingstage == STAGE_NORMALSPEED && T1 > 0.){ // check need of T1 + if(t >= T1){ + DBG("T1=%g, t=%g -->", T1, t); + curparams.speed = (curparams.speed > 0.) ? Min.speed : -Min.speed; + movingstage = STAGE_MINSPEED; + Xlast0 = curparams.coord; + T0 = T1; + DBG("Go further with minimal speed"); + } + } + if(fabs(curparams.coord - target.coord) < coord_tolerance){ // we are at place + DBG("OK, we are in place"); + stop(t); + } + if(next) *next = curparams; + return state; +} + +static movestate_t getst(moveparam_t *cur){ + if(cur) *cur = curparams; + return state; +} + +movemodel_t dumb = { + .init_limits = initlims, + .calculate = calc, + .proc_move = proc, + .stop = stop, + .emergency_stop = stop, + .get_state = getst, +}; diff --git a/Auxiliary_utils/LibSidServo/moving_model/Dramp.h b/Auxiliary_utils/LibSidServo/moving_model/Dramp.h new file mode 100644 index 0000000..e765c77 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/Dramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model 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 "moving_private.h" + +extern movemodel_t dumb; diff --git a/Auxiliary_utils/LibSidServo/moving_model/Makefile b/Auxiliary_utils/LibSidServo/moving_model/Makefile new file mode 100644 index 0000000..5e19d5d --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/Makefile @@ -0,0 +1,57 @@ +# run `make DEF=...` to add extra defines +PROGRAM := moving +LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all +LDFLAGS += -lusefull_macros -lm +SRCS := $(wildcard *.c) +DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111 +OBJDIR := mk +CFLAGS += -O2 -Wall -Wextra -Wno-trampolines -std=gnu99 +OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o)) +DEPS := $(OBJS:.o=.d) +TARGFILE := $(OBJDIR)/TARGET +CC = gcc +#TARGET := RELEASE + +ifeq ($(shell test -e $(TARGFILE) && echo -n yes),yes) + TARGET := $(file < $(TARGFILE)) +else + TARGET := RELEASE +endif + +ifeq ($(TARGET), DEBUG) + .DEFAULT_GOAL := debug +endif + +release: $(PROGRAM) + +debug: CFLAGS += -DEBUG -Werror +debug: TARGET := DEBUG +debug: $(PROGRAM) + +$(TARGFILE): $(OBJDIR) + @echo -e "\t\tTARGET: $(TARGET)" + @echo "$(TARGET)" > $(TARGFILE) + +$(PROGRAM) : $(TARGFILE) $(OBJS) + @echo -e "\t\tLD $(PROGRAM)" + $(CC) $(OBJS) $(LDFLAGS) -o $(PROGRAM) + +$(OBJDIR): + @mkdir $(OBJDIR) + +ifneq ($(MAKECMDGOALS),clean) +-include $(DEPS) +endif + +$(OBJDIR)/%.o: %.c + @echo -e "\t\tCC $<" + $(CC) $< -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@ + +clean: + @echo -e "\t\tCLEAN" + @rm -rf $(OBJDIR) 2>/dev/null || true + +xclean: clean + @rm -f $(PROGRAM) + +.PHONY: clean xclean diff --git a/Auxiliary_utils/LibSidServo/moving_model/Sramp.c b/Auxiliary_utils/LibSidServo/moving_model/Sramp.c new file mode 100644 index 0000000..47f03ed --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/Sramp.c @@ -0,0 +1,27 @@ +/* + * This file is part of the moving_model 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 . + */ + +// quasi non-jerk s-ramp + +#include +#include + +#include "Sramp.h" + + +movemodel_t s_shaped = { 0 }; diff --git a/Auxiliary_utils/LibSidServo/moving_model/Sramp.h b/Auxiliary_utils/LibSidServo/moving_model/Sramp.h new file mode 100644 index 0000000..3decd8b --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/Sramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model 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 "moving_private.h" + +extern movemodel_t s_shaped; diff --git a/Auxiliary_utils/LibSidServo/moving_model/Tramp.c b/Auxiliary_utils/LibSidServo/moving_model/Tramp.c new file mode 100644 index 0000000..ea26f88 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/Tramp.c @@ -0,0 +1,217 @@ +/* + * This file is part of the moving_model 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 . + */ + +// simplest trapezioidal ramp + +#include +#include +#include +#include + +#include "Tramp.h" + +static movestate_t state = ST_STOP; +static moveparam_t Min, Max; // `Min` acceleration not used! + +typedef enum{ + STAGE_ACCEL, // start from zero speed and accelerate to Max speed + STAGE_MAXSPEED, // go with target speed + STAGE_DECEL, // go from target speed to zero + STAGE_STOPPED, // stop + STAGE_AMOUNT +} movingstage_t; + +static movingstage_t movingstage = STAGE_STOPPED; +static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts +static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage +static moveparam_t curparams = {0}; // current coordinate/speed/acceleration + +static int initlims(limits_t *lim){ + if(!lim) return FALSE; + Min = lim->min; + Max = lim->max; + return TRUE; +} + +static void emstop(double _U_ t){ + curparams.accel = 0.; + curparams.speed = 0.; + bzero(Times, sizeof(Times)); + bzero(Params, sizeof(Params)); + state = ST_STOP; + movingstage = STAGE_STOPPED; +} + +static void stop(double t){ + if(state == ST_STOP || movingstage == STAGE_STOPPED) return; + movingstage = STAGE_DECEL; + state = ST_MOVE; + Times[STAGE_DECEL] = t; + Params[STAGE_DECEL].speed = curparams.speed; + if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel; + else Params[STAGE_DECEL].accel = Max.accel; + Params[STAGE_DECEL].coord = curparams.coord; + // speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2 + Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel; + // coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2 + double dt = Times[STAGE_STOPPED] - t; + Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt + + Params[STAGE_DECEL].accel * dt * dt / 2.; +} + +/** + * @brief calc - moving calculation + * @param x - using max speed (>0!!!) and coordinate + * @param t - current time value + * @return FALSE if can't move with given parameters + */ +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; + 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; + if(state == ST_STOP) dx01 = setspeed * dt01 / 2.; + else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed); + DBG("dx01=%g, dt01=%g", dx01, dt01); + }else{ // increase or decrease speed without stopping phase + dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; + 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(dx01 + dx23 > Dx){ // calculate max speed + setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.); + if(setspeed < curspeed){ + setspeed = curparams.speed; + dt01 = 0.; dx01 = 0.; + Params[0].accel = 0.; + }else{ + Params[0].accel = a; + dt01 = fabs(setspeed - curspeed) / Max.accel; + dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.; + } + }else Params[0].accel = a; + } + if(setspeed < Min.speed){ + DBG("New speed should be too small"); + return FALSE; + } + moveparam_t *p = &Params[STAGE_MAXSPEED]; + p->accel = 0.; p->speed = sign * setspeed; + p->coord = curparams.coord + dx01 * sign; + Times[STAGE_MAXSPEED] = Times[0] + dt01; + 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 = sign * setspeed; + p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12; + Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12; + p = &Params[STAGE_STOPPED]; + p->accel = 0.; p->speed = 0.; p->coord = x->coord; + Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23; + for(int i = 0; i < 4; ++i) + DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i, + Times[i], Params[i].coord, Params[i].speed, Params[i].accel); + state = ST_MOVE; + movingstage = STAGE_ACCEL; + return TRUE; +} + +static movestate_t proc(moveparam_t *next, double t){ + if(state == ST_STOP) goto ret; + for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){ + if(Times[s] <= t){ // check time for current stage + movingstage = s; + break; + } + } + if(movingstage == STAGE_STOPPED){ + curparams.coord = Params[STAGE_STOPPED].coord; + emstop(t); + goto ret; + } + // calculate current parameters + double dt = t - Times[movingstage]; + double a = Params[movingstage].accel; + double v0 = Params[movingstage].speed; + double x0 = Params[movingstage].coord; + curparams.accel = a; + curparams.speed = v0 + a * dt; + curparams.coord = x0 + v0 * dt + a * dt * dt / 2.; +ret: + if(next) *next = curparams; + return state; +} + +static movestate_t getst(moveparam_t *cur){ + if(cur) *cur = curparams; + return state; +} + +static double gettstop(){ + return Times[STAGE_STOPPED]; +} + +movemodel_t trapez = { + .init_limits = initlims, + .stop = stop, + .emergency_stop = emstop, + .get_state = getst, + .calculate = calc, + .proc_move = proc, + .stoppedtime = gettstop, +}; diff --git a/Auxiliary_utils/LibSidServo/moving_model/Tramp.h b/Auxiliary_utils/LibSidServo/moving_model/Tramp.h new file mode 100644 index 0000000..ea4a257 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/Tramp.h @@ -0,0 +1,23 @@ +/* + * This file is part of the moving_model 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 "moving_private.h" + +extern movemodel_t trapez; diff --git a/Auxiliary_utils/LibSidServo/moving_model/main.c b/Auxiliary_utils/LibSidServo/moving_model/main.c new file mode 100644 index 0000000..f6b8e6b --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/main.c @@ -0,0 +1,124 @@ +/* + * This file is part of the moving_model 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 "moving.h" + +static movemodel_t *model = NULL; +static FILE *coordslog = NULL; +static double Tstart = 0.; + +typedef struct{ + int help; + char *ramptype; + char *xlog; + int dT; +} pars; + +static pars G = { + .ramptype = "d", + .dT = 10000, +}; + +static limits_t limits = { + .min = {.coord = -1e6, .speed = 0.1, .accel = 0.1}, + .max = {.coord = 1e6, .speed = 1e3, .accel = 50.}, + .jerk = 10. +}; + +static myoption opts[] = { + {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, + {"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"}, + {"deltat", NEED_ARG, NULL, 't', arg_int, APTR(&G.dT), "time interval for monitoring (microseconds, >0)"}, + {"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"}, + // TODO: add parameters for limits setting + end_option +}; + +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_MOVE) printf("Current state: moving; recalculte new parameters\n"); + if(!move_to(tag)){ + WARNX("move(): can't move to %g with max speed %g", tag->coord, tag->speed); + return FALSE; + } + green("Moving from %g (speed=%g, acc=%g) to %g with maximal speed %g\n", + curpos.coord, curpos.speed, curpos.accel, tag->coord, tag->speed); + return TRUE; +} + +// monitor moving with dump to file until T-Tnow == tnext or stop/error +// show position every dT +static void monit(double tnext){ + DBG("start monitoring"); + double t0 = nanot(), t = 0.; + do{ + t = nanot(); + moveparam_t p; + 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) break; + usleep(G.dT); + }while(nanot() - t0 < tnext); + DBG("End of monitoring"); +} + +int main(int argc, char **argv){ + initial_setup(); + parseargs(&argc, &argv, opts); + if(G.help) showhelp(-1, opts); + if(G.xlog){ + coordslog = fopen(G.xlog, "w"); + if(!coordslog) ERR("Can't open %s", G.xlog); + } else coordslog = stdout; + if(G.dT < 1) G.dT = 1; + fprintf(coordslog, "time coordinate speed acceleration\n"); + ramptype_t ramp = RAMP_AMOUNT; + if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB; + else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM; + else if(*G.ramptype == 's' || *G.ramptype == 'S') ramp = RAMP_S; + else ERRX("Point \"d\" (dumb), \"s\" (s-type), or \"t\" (trapez) for ramp type"); + model = init_moving(ramp, &limits); + if(!model) ERRX("Can't init moving model: check parameters"); + Tstart = nanot(); + moveparam_t target = {.speed = 10., .coord = 20.}; + if(move(&target)) monit(0.5); + for(int i = 0; i < 10; ++i){ + target.coord = -target.coord; + if(move(&target)) monit(1.); + } + target.coord = 0.; target.speed = 20.; + if(move(&target)) monit(100.); + return 0; + if(move(&target)) monit(1.); + target.speed = 15.; + target.coord = 30.; + if(move(&target)) monit(1.); + target.coord = 0.; + if(move(&target)) monit(1.5); + target.coord = 0.; target.speed = 20.; + if(move(&target)) monit(1e6); + usleep(5000); + fclose(coordslog); + return 0; +} diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving.c b/Auxiliary_utils/LibSidServo/moving_model/moving.c new file mode 100644 index 0000000..41f6cde --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving.c @@ -0,0 +1,125 @@ +/* + * This file is part of the moving_model 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 +#include +#include + +#include "moving.h" +#include "moving_private.h" +#include "Dramp.h" +#include "Sramp.h" +#include "Tramp.h" + +//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +static movemodel_t *model = NULL; +double coord_tolerance = COORD_TOLERANCE_DEFAULT; +double time_tick = TIME_TICK_DEFAULT; + +// difference of time from first call, using nanoseconds +double nanot(){ + static struct timespec *start = NULL; + struct timespec now; + if(!start){ + start = MALLOC(struct timespec, 1); + if(!start) return -1.; + if(clock_gettime(CLOCK_REALTIME, start)) return -1.; + } + if(clock_gettime(CLOCK_REALTIME, &now)) return -1.; + //DBG("was: %ld, now: %ld", start->tv_nsec, now.tv_nsec); + 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; +} + +static void* thread(void _U_ *u){ + if(!model) return NULL; + DBG("START thread"); + double t = 0.; + while(1){ + t = nanot(); + //pthread_mutex_lock(&mutex); + movestate_t curstate = model->get_state(NULL); + moveparam_t curmove; + if(curstate == ST_MOVE){ + curstate = model->proc_move(&curmove, t); + } + //pthread_mutex_unlock(&mutex); + while(nanot() - t < time_tick); + } + return NULL; +} + +static void chkminmax(double *min, double *max){ + if(*min <= *max) return; + double t = *min; + *min = *max; + *max = t; +} + +movemodel_t *init_moving(ramptype_t type, limits_t *l){ + if(!l) return FALSE; + pthread_t t; + switch(type){ + case RAMP_DUMB: + model = &dumb; + break; + case RAMP_TRAPEZIUM: + model = &trapez; + break; + case RAMP_S: + model = &s_shaped; + break; + default: + return FALSE; + } + if(!model->init_limits) return NULL; + moveparam_t *max = &l->max, *min = &l->min; + if(min->speed < 0.) min->speed = -min->speed; + if(max->speed < 0.) max->speed = -max->speed; + if(min->accel < 0.) min->accel = -min->accel; + if(max->accel < 0.) max->accel = -max->accel; + chkminmax(&min->coord, &max->coord); + chkminmax(&min->speed, &max->speed); + chkminmax(&min->accel, &max->accel); + if(!model->init_limits(l)) return NULL; + if(pthread_create(&t, NULL, thread, NULL)) return NULL; + pthread_detach(t); + return model; +} + +int move_to(moveparam_t *target){ + if(!target || !model) return FALSE; + // only positive velocity + if(target->speed < 0.) target->speed = -target->speed; + // don't mind about acceleration - user cannot set it now + return model->calculate(target, nanot()); +} + +int init_coordtol(double tolerance){ + if(tolerance < COORD_TOLERANCE_MIN || tolerance > COORD_TOLERANCE_MAX) return FALSE; + coord_tolerance = tolerance; + return TRUE; +} +int init_timetick(double tick){ + if(tick < TIME_TICK_MIN || tick > TIME_TICK_MAX) return FALSE; + time_tick = tick; + return TRUE; +} diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving.h b/Auxiliary_utils/LibSidServo/moving_model/moving.h new file mode 100644 index 0000000..a0f6d62 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving.h @@ -0,0 +1,70 @@ +/* + * This file is part of the moving_model 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 + +// tolerance, time ticks +#define COORD_TOLERANCE_DEFAULT (0.01) +#define COORD_TOLERANCE_MIN (0.0001) +#define COORD_TOLERANCE_MAX (10.) +#define TIME_TICK_DEFAULT (0.0001) +#define TIME_TICK_MIN (1e-9) +#define TIME_TICK_MAX (10.) + +typedef enum{ + RAMP_DUMB, // no ramp: infinite acceleration/deceleration + RAMP_TRAPEZIUM, // trapezium ramp + RAMP_S, // s-shaped ramp + RAMP_AMOUNT +} ramptype_t; + +typedef enum{ + ST_STOP, // stopped + ST_MOVE, // moving + ST_AMOUNT +} movestate_t; + +typedef struct{ // all values could be both as positive and negative + double coord; + double speed; + double accel; +} moveparam_t; + +typedef struct{ + moveparam_t min; + moveparam_t max; + double jerk; +} limits_t; + +typedef struct{ + int (*init_limits)(limits_t *lim); // init values of limits, jerk + int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t + movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t + movestate_t (*get_state)(moveparam_t *cur); // get current moving state + void (*stop)(double t); // stop by ramp + void (*emergency_stop)(double t); // stop with highest acceleration + double (*stoppedtime)(); // time when moving will ends +} movemodel_t; + +extern double coord_tolerance; + +double nanot(); +movemodel_t *init_moving(ramptype_t type, limits_t *l); +int init_coordtol(double tolerance); +int init_timetick(double tick); +int move_to(moveparam_t *target); diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving_model.cflags b/Auxiliary_utils/LibSidServo/moving_model/moving_model.cflags new file mode 100644 index 0000000..68d5165 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving_model.cflags @@ -0,0 +1 @@ +-std=c17 \ No newline at end of file diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving_model.config b/Auxiliary_utils/LibSidServo/moving_model/moving_model.config new file mode 100644 index 0000000..cadc51b --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving_model.config @@ -0,0 +1,4 @@ +// Add predefined macros for your project here. For example: +// #define THE_ANSWER 42 +#define _XOPEN_SOURCE 666 +#define EBUG diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving_model.creator b/Auxiliary_utils/LibSidServo/moving_model/moving_model.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving_model.creator @@ -0,0 +1 @@ +[General] diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving_model.creator.user b/Auxiliary_utils/LibSidServo/moving_model/moving_model.creator.user new file mode 100644 index 0000000..2c3598d --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving_model.creator.user @@ -0,0 +1,185 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 1 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 1 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + Desktop + {91347f2c-5221-46a7-80b1-0a054ca02f79} + 0 + 0 + 0 + + /home/eddy/C-files/mountcontrol.git/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Сборка + Сборка + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Очистка + Очистка + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + По умолчанию + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Развёртывание + Развёртывание + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving_model.creator.user.7bd84e3 b/Auxiliary_utils/LibSidServo/moving_model/moving_model.creator.user.7bd84e3 new file mode 100644 index 0000000..1311ab3 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving_model.creator.user.7bd84e3 @@ -0,0 +1,184 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 0 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 8 + true + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Small_tel/moving_model + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving_model.cxxflags b/Auxiliary_utils/LibSidServo/moving_model/moving_model.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving_model.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving_model.files b/Auxiliary_utils/LibSidServo/moving_model/moving_model.files new file mode 100644 index 0000000..e522e0e --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving_model.files @@ -0,0 +1,10 @@ +Dramp.c +Dramp.h +Sramp.c +Sramp.h +Tramp.c +Tramp.h +main.c +moving.c +moving.h +moving_private.h diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving_model.includes b/Auxiliary_utils/LibSidServo/moving_model/moving_model.includes new file mode 100644 index 0000000..e69de29 diff --git a/Auxiliary_utils/LibSidServo/moving_model/moving_private.h b/Auxiliary_utils/LibSidServo/moving_model/moving_private.h new file mode 100644 index 0000000..e62a8dd --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/moving_private.h @@ -0,0 +1,26 @@ +/* + * This file is part of the moving_model 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 "moving.h" + +extern double coord_tolerance; +extern double time_tick; + + diff --git a/Auxiliary_utils/LibSidServo/moving_model/plot b/Auxiliary_utils/LibSidServo/moving_model/plot new file mode 100755 index 0000000..f6c9490 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/plot @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +#set term pdf +#set output "output.pdf" +plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader +pause mouse diff --git a/Auxiliary_utils/LibSidServo/moving_model/plot_pdf b/Auxiliary_utils/LibSidServo/moving_model/plot_pdf new file mode 100755 index 0000000..2001ca0 --- /dev/null +++ b/Auxiliary_utils/LibSidServo/moving_model/plot_pdf @@ -0,0 +1,5 @@ +#!/usr/bin/gnuplot + +set term pdf +set output "output.pdf" +plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader diff --git a/Auxiliary_utils/LibSidServo/movingfilter.c b/Auxiliary_utils/LibSidServo/movingfilter.c new file mode 100644 index 0000000..16001b2 --- /dev/null +++ b/Auxiliary_utils/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