From 502014bee43eb2847c27624c47621cd45e178d1d Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Wed, 30 Jul 2025 13:43:39 +0300 Subject: [PATCH] PID test almost in ssii format --- .../{PID_test => PID_test.deprecated}/Dramp.c | 0 .../{PID_test => PID_test.deprecated}/Dramp.h | 0 LibSidServo/PID_test.deprecated/Makefile | 57 ++++ .../{PID_test => PID_test.deprecated}/Sramp.c | 0 .../{PID_test => PID_test.deprecated}/Sramp.h | 0 LibSidServo/PID_test.deprecated/Tramp.c | 223 ++++++++++++++++ LibSidServo/PID_test.deprecated/Tramp.h | 23 ++ LibSidServo/PID_test.deprecated/main.c | 243 ++++++++++++++++++ LibSidServo/PID_test.deprecated/moving.c | 105 ++++++++ LibSidServo/PID_test.deprecated/moving.h | 70 +++++ .../PID_test.deprecated/moving_model.cflags | 1 + .../PID_test.deprecated/moving_model.config | 4 + .../PID_test.deprecated/moving_model.creator | 1 + .../moving_model.creator.user | 221 ++++++++++++++++ .../moving_model.creator.user.7bd84e3 | 184 +++++++++++++ .../PID_test.deprecated/moving_model.cxxflags | 1 + .../PID_test.deprecated/moving_model.files | 10 + .../PID_test.deprecated/moving_model.includes | 0 .../PID_test.deprecated/moving_private.h | 26 ++ LibSidServo/PID_test.deprecated/plot | 4 + LibSidServo/PID_test.deprecated/plot.cont | 8 + LibSidServo/PID_test.deprecated/plot_jpg | 6 + LibSidServo/PID_test.deprecated/plot_pdf | 5 + LibSidServo/PID_test.deprecated/plotacc | 4 + LibSidServo/PID_test.deprecated/ploterr | 4 + LibSidServo/PID_test.deprecated/ploterr.cont | 6 + LibSidServo/PID_test.deprecated/ploterr_jpg | 5 + LibSidServo/PID_test/PID.c | 64 +++++ LibSidServo/PID_test/PID.h | 39 +++ LibSidServo/PID_test/main.c | 145 +++++------ LibSidServo/PID_test/moving.c | 20 +- LibSidServo/PID_test/moving.h | 10 +- LibSidServo/PID_test/moving_model.files | 2 + 33 files changed, 1381 insertions(+), 110 deletions(-) rename LibSidServo/{PID_test => PID_test.deprecated}/Dramp.c (100%) rename LibSidServo/{PID_test => PID_test.deprecated}/Dramp.h (100%) create mode 100644 LibSidServo/PID_test.deprecated/Makefile rename LibSidServo/{PID_test => PID_test.deprecated}/Sramp.c (100%) rename LibSidServo/{PID_test => PID_test.deprecated}/Sramp.h (100%) create mode 100644 LibSidServo/PID_test.deprecated/Tramp.c create mode 100644 LibSidServo/PID_test.deprecated/Tramp.h create mode 100644 LibSidServo/PID_test.deprecated/main.c create mode 100644 LibSidServo/PID_test.deprecated/moving.c create mode 100644 LibSidServo/PID_test.deprecated/moving.h create mode 100644 LibSidServo/PID_test.deprecated/moving_model.cflags create mode 100644 LibSidServo/PID_test.deprecated/moving_model.config create mode 100644 LibSidServo/PID_test.deprecated/moving_model.creator create mode 100644 LibSidServo/PID_test.deprecated/moving_model.creator.user create mode 100644 LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 create mode 100644 LibSidServo/PID_test.deprecated/moving_model.cxxflags create mode 100644 LibSidServo/PID_test.deprecated/moving_model.files create mode 100644 LibSidServo/PID_test.deprecated/moving_model.includes create mode 100644 LibSidServo/PID_test.deprecated/moving_private.h create mode 100755 LibSidServo/PID_test.deprecated/plot create mode 100755 LibSidServo/PID_test.deprecated/plot.cont create mode 100755 LibSidServo/PID_test.deprecated/plot_jpg create mode 100755 LibSidServo/PID_test.deprecated/plot_pdf create mode 100755 LibSidServo/PID_test.deprecated/plotacc create mode 100755 LibSidServo/PID_test.deprecated/ploterr create mode 100755 LibSidServo/PID_test.deprecated/ploterr.cont create mode 100755 LibSidServo/PID_test.deprecated/ploterr_jpg create mode 100644 LibSidServo/PID_test/PID.c create mode 100644 LibSidServo/PID_test/PID.h diff --git a/LibSidServo/PID_test/Dramp.c b/LibSidServo/PID_test.deprecated/Dramp.c similarity index 100% rename from LibSidServo/PID_test/Dramp.c rename to LibSidServo/PID_test.deprecated/Dramp.c diff --git a/LibSidServo/PID_test/Dramp.h b/LibSidServo/PID_test.deprecated/Dramp.h similarity index 100% rename from LibSidServo/PID_test/Dramp.h rename to LibSidServo/PID_test.deprecated/Dramp.h diff --git a/LibSidServo/PID_test.deprecated/Makefile b/LibSidServo/PID_test.deprecated/Makefile new file mode 100644 index 0000000..5e19d5d --- /dev/null +++ b/LibSidServo/PID_test.deprecated/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/LibSidServo/PID_test/Sramp.c b/LibSidServo/PID_test.deprecated/Sramp.c similarity index 100% rename from LibSidServo/PID_test/Sramp.c rename to LibSidServo/PID_test.deprecated/Sramp.c diff --git a/LibSidServo/PID_test/Sramp.h b/LibSidServo/PID_test.deprecated/Sramp.h similarity index 100% rename from LibSidServo/PID_test/Sramp.h rename to LibSidServo/PID_test.deprecated/Sramp.h diff --git a/LibSidServo/PID_test.deprecated/Tramp.c b/LibSidServo/PID_test.deprecated/Tramp.c new file mode 100644 index 0000000..f164463 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/Tramp.c @@ -0,0 +1,223 @@ +/* + * 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" + +#undef DBG +#define DBG(...) + +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){ + WARNX("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 = sign * Max.accel; + if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;} + else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;} + //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){ + WARNX("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 < -coord_tolerance){ + WARNX("Oops, WTF dx12=%g?", dx12); + 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/LibSidServo/PID_test.deprecated/Tramp.h b/LibSidServo/PID_test.deprecated/Tramp.h new file mode 100644 index 0000000..ea4a257 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/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/LibSidServo/PID_test.deprecated/main.c b/LibSidServo/PID_test.deprecated/main.c new file mode 100644 index 0000000..5a67b97 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/main.c @@ -0,0 +1,243 @@ +/* + * 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 "moving.h" + +// errors for states: slewing/pointing/guiding +#define MAX_POINTING_ERR (50.) +#define MAX_GUIDING_ERR (5.) +// timeout to "forget" old data from I sum array; seconds +#define PID_I_PERIOD (3.) + +static movemodel_t *model = NULL; +static FILE *coordslog = NULL; + +typedef enum{ + Slewing, + Pointing, + Guiding +} state_t; + +static state_t state = Slewing; + +typedef struct{ + int help; + char *ramptype; + char *xlog; + double dTmon; + double dTcorr; + double Tend; + double minerr; + double P, I, D; +} pars; + +static pars G = { + .ramptype = "t", + .dTmon = 0.01, + .dTcorr = 0.05, + .Tend = 100., + .minerr = 0.1, + .P = 0.8, +}; + +static limits_t limits = { + .min = {.coord = -1e6, .speed = 0.01, .accel = 0.1}, + .max = {.coord = 1e6, .speed = 1e3, .accel = 500.}, + .jerk = 10. +}; + +typedef struct { + double kp, ki, kd; // PID gains + double prev_error; // Previous error + double integral; // Integral term + double *pidIarray; // array for Integral + size_t pidIarrSize; // it's size + size_t curIidx; // and index of current element +} PIDController; + +static PIDController pid; + +static sl_option_t 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"}, + {"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"}, + {"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"}, + {"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"}, + {"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"}, + {"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"}, + {"prop", NEED_ARG, NULL, 'P', arg_double, APTR(&G.P), "P-coefficient of PID"}, + {"integ", NEED_ARG, NULL, 'I', arg_double, APTR(&G.I), "I-coefficient of PID"}, + {"diff", NEED_ARG, NULL, 'D', arg_double, APTR(&G.D), "D-coefficient of PID"}, + // TODO: add parameters for limits setting + end_option +}; + +// calculate coordinate target for given time (starting from zero) +static double target_coord(double t){ + if(t > 20. && t < 30.) return target_coord(20.); + double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5); + return pos; +} + +/* P-only == oscillations +static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){ + double error = targcoord - p->coord; + if(fabs(error) < G.minerr) return p->speed; + return p->speed + error / dt / 500.; +} +*/ + +static void pid_init(PIDController *pid, double kp, double ki, double kd) { + pid->kp = fabs(kp); + pid->ki = fabs(ki); + pid->kd = fabs(kd); + pid->prev_error = 0.; + pid->integral = 0.; + pid->curIidx = 0; + pid->pidIarrSize = PID_I_PERIOD / G.dTcorr; + if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements"); + pid->pidIarray = MALLOC(double, pid->pidIarrSize); +} + +static void pid_clear(PIDController *pid){ + if(!pid) return; + bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize); + pid->integral = 0.; + pid->prev_error = 0.; + pid->curIidx = 0; +} + +static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){ + double error = targcoord - p->coord, fe = fabs(error); + switch(state){ + case Slewing: + if(fe < MAX_POINTING_ERR){ + pid_clear(&pid); + state = Pointing; + green("--> Pointing\n"); + }else{ + red("Slewing...\n"); + return (error > 0.) ? limits.max.speed : -limits.max.speed; + } + break; + case Pointing: + if(fe < MAX_GUIDING_ERR){ + pid_clear(&pid); + state = Guiding; + green("--> Guiding\n"); + }else if(fe > MAX_POINTING_ERR){ + red("--> Slewing\n"); + state = Slewing; + return (error > 0.) ? limits.max.speed : -limits.max.speed; + } + break; + case Guiding: + if(fe > MAX_GUIDING_ERR){ + red("--> Pointing\n"); + state = Pointing; + }else if(fe < G.minerr){ + green("At target\n"); + //pid_clear(&pid); + //return p->speed; + } + break; + } + + red("Calculate PID\n"); + double oldi = pid.pidIarray[pid.curIidx], newi = error * dt; + pid.pidIarray[pid.curIidx++] = oldi; + if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0; + pid.integral += newi - oldi; + double derivative = (error - pid.prev_error) / dt; + pid.prev_error = error; + DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative); + double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative); + if(state == Pointing) add /= 3.; + else if(state == Guiding) add /= 7.; + DBG("ADD = %g; new speed = %g", add, p->speed + add); + if(state == Guiding) return p->speed + add / dt / 10.; + return add / dt; +} +// ./moving -l coords -P.5 -I.05 -D1.5 +// ./moving -l coords -P1.3 -D1.6 + +static void start_model(double Tend){ + double T = 0., Tcorr = 0.;//, Tlast = 0.; + moveparam_t target; + while(T <= Tend){ + moveparam_t p; + movestate_t st = model->get_state(&p); + if(st == ST_MOVE) st = model->proc_move(&p, T); + double nextcoord = target_coord(T); + double error = nextcoord - p.coord; + if(T - Tcorr >= G.dTcorr){ // check correction + double speed = getNewSpeed(&p, nextcoord, T - Tcorr); + target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5; + target.speed = fabs(speed); + double res_speed = limits.max.speed / 2.; + if(target.speed > limits.max.speed){ + target.speed = limits.max.speed; + res_speed = limits.max.speed / 4.; + }else if(target.speed < limits.min.speed){ + target.speed = limits.min.speed; + res_speed = limits.min.speed * 4.; + } + if(!move_to(&target, T)){ + target.speed = res_speed; + if(!move_to(&target, T)) + WARNX("move(): can't move to %g with max speed %g", target.coord, target.speed); + } + DBG("%g: tag/cur speed= %g / %g; tag/cur pos = %g / %g; err = %g", T, target.speed, p.speed, target.coord, p.coord, error); + Tcorr = T; + } + // make log + fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\n", + T, nextcoord, p.coord, p.speed, p.accel, error); + T += G.dTmon; + } +} + +int main(int argc, char **argv){ + sl_init(); + sl_parseargs(&argc, &argv, opts); + if(G.help) sl_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.dTmon <= 0.) ERRX("tmon should be > 0."); + if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1."); + if(G.Tend <= 0.) ERRX("tend should be > 0."); + pid_init(&pid, G.P, G.I, G.D); + fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error"); + 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"); + start_model(G.Tend); + fclose(coordslog); + return 0; +} diff --git a/LibSidServo/PID_test.deprecated/moving.c b/LibSidServo/PID_test.deprecated/moving.c new file mode 100644 index 0000000..d7264c0 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving.c @@ -0,0 +1,105 @@ +/* + * 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 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; + 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; + return model; +} + +int move_to(moveparam_t *target, double t){ + if(!target || !model) return FALSE; + DBG("MOVE to %g at speed %g", target->coord, target->speed); + // 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, t); +} + +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/LibSidServo/PID_test.deprecated/moving.h b/LibSidServo/PID_test.deprecated/moving.h new file mode 100644 index 0000000..6854308 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/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, double t); diff --git a/LibSidServo/PID_test.deprecated/moving_model.cflags b/LibSidServo/PID_test.deprecated/moving_model.cflags new file mode 100644 index 0000000..68d5165 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.cflags @@ -0,0 +1 @@ +-std=c17 \ No newline at end of file diff --git a/LibSidServo/PID_test.deprecated/moving_model.config b/LibSidServo/PID_test.deprecated/moving_model.config new file mode 100644 index 0000000..cadc51b --- /dev/null +++ b/LibSidServo/PID_test.deprecated/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/LibSidServo/PID_test.deprecated/moving_model.creator b/LibSidServo/PID_test.deprecated/moving_model.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.creator @@ -0,0 +1 @@ +[General] diff --git a/LibSidServo/PID_test.deprecated/moving_model.creator.user b/LibSidServo/PID_test.deprecated/moving_model.creator.user new file mode 100644 index 0000000..eaf30ec --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.creator.user @@ -0,0 +1,221 @@ + + + + + + EnvironmentId + {cf63021e-ef53-49b0-b03b-2f2570cdf3b6} + + + 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 + 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 + true + 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 + 0 + 0 + + + 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 + + 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/LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 b/LibSidServo/PID_test.deprecated/moving_model.creator.user.7bd84e3 new file mode 100644 index 0000000..1311ab3 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/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/LibSidServo/PID_test.deprecated/moving_model.cxxflags b/LibSidServo/PID_test.deprecated/moving_model.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/LibSidServo/PID_test.deprecated/moving_model.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/LibSidServo/PID_test.deprecated/moving_model.files b/LibSidServo/PID_test.deprecated/moving_model.files new file mode 100644 index 0000000..e522e0e --- /dev/null +++ b/LibSidServo/PID_test.deprecated/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/LibSidServo/PID_test.deprecated/moving_model.includes b/LibSidServo/PID_test.deprecated/moving_model.includes new file mode 100644 index 0000000..e69de29 diff --git a/LibSidServo/PID_test.deprecated/moving_private.h b/LibSidServo/PID_test.deprecated/moving_private.h new file mode 100644 index 0000000..e62a8dd --- /dev/null +++ b/LibSidServo/PID_test.deprecated/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/LibSidServo/PID_test.deprecated/plot b/LibSidServo/PID_test.deprecated/plot new file mode 100755 index 0000000..476e1f3 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/plot @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader +pause mouse diff --git a/LibSidServo/PID_test.deprecated/plot.cont b/LibSidServo/PID_test.deprecated/plot.cont new file mode 100755 index 0000000..16dc0f5 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/plot.cont @@ -0,0 +1,8 @@ +#!/usr/bin/gnuplot + +#set term pdf +#set output "output.pdf" +while(1){ + plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader + pause 1 +} \ No newline at end of file diff --git a/LibSidServo/PID_test.deprecated/plot_jpg b/LibSidServo/PID_test.deprecated/plot_jpg new file mode 100755 index 0000000..f926de6 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/plot_jpg @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +set terminal jpeg size 1000,500 +set output "all.jpg" + +plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader diff --git a/LibSidServo/PID_test.deprecated/plot_pdf b/LibSidServo/PID_test.deprecated/plot_pdf new file mode 100755 index 0000000..2001ca0 --- /dev/null +++ b/LibSidServo/PID_test.deprecated/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/LibSidServo/PID_test.deprecated/plotacc b/LibSidServo/PID_test.deprecated/plotacc new file mode 100755 index 0000000..f15faaa --- /dev/null +++ b/LibSidServo/PID_test.deprecated/plotacc @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:5 with lines title columnheader +pause mouse diff --git a/LibSidServo/PID_test.deprecated/ploterr b/LibSidServo/PID_test.deprecated/ploterr new file mode 100755 index 0000000..d2b990d --- /dev/null +++ b/LibSidServo/PID_test.deprecated/ploterr @@ -0,0 +1,4 @@ +#!/usr/bin/gnuplot + +plot 'coords' using 1:6 with lines title columnheader +pause mouse diff --git a/LibSidServo/PID_test.deprecated/ploterr.cont b/LibSidServo/PID_test.deprecated/ploterr.cont new file mode 100755 index 0000000..95bcd4b --- /dev/null +++ b/LibSidServo/PID_test.deprecated/ploterr.cont @@ -0,0 +1,6 @@ +#!/usr/bin/gnuplot + +while(1){ + plot 'coords' using 1:6 with lines title columnheader + pause 1 +} diff --git a/LibSidServo/PID_test.deprecated/ploterr_jpg b/LibSidServo/PID_test.deprecated/ploterr_jpg new file mode 100755 index 0000000..b1bd63b --- /dev/null +++ b/LibSidServo/PID_test.deprecated/ploterr_jpg @@ -0,0 +1,5 @@ +#!/usr/bin/gnuplot + +set term jpeg size 1000,500 +set output "error.jpg" +plot 'coords' using 1:6 with lines title columnheader diff --git a/LibSidServo/PID_test/PID.c b/LibSidServo/PID_test/PID.c new file mode 100644 index 0000000..7774258 --- /dev/null +++ b/LibSidServo/PID_test/PID.c @@ -0,0 +1,64 @@ +/* + * 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 "PID.h" + +PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz){ + if(!gain || Iarrsz < 3) return NULL; + PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t)); + pid->gain = *gain; + pid->pidIarrSize = Iarrsz; + pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double)); + return pid; +} + +void pid_clear(PIDController_t *pid){ + if(!pid) return; + DBG("CLEAR PID PARAMETERS"); + bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize); + pid->integral = 0.; + pid->prev_error = 0.; + pid->curIidx = 0; +} + +void pid_delete(PIDController_t **pid){ + if(!pid || !*pid) return; + if((*pid)->pidIarray) free((*pid)->pidIarray); + free(*pid); + *pid = NULL; +} + +double pid_calculate(PIDController_t *pid, double error, double dt){ + // calculate flowing integral + double oldi = pid->pidIarray[pid->curIidx], newi = error * dt; + DBG("oldi/new: %g, %g", oldi, newi); + pid->pidIarray[pid->curIidx++] = newi; + if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0; + pid->integral += newi - oldi; + double derivative = (error - pid->prev_error) / dt; + pid->prev_error = error; + double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative; + DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum); + return sum; +} diff --git a/LibSidServo/PID_test/PID.h b/LibSidServo/PID_test/PID.h new file mode 100644 index 0000000..e60ddd6 --- /dev/null +++ b/LibSidServo/PID_test/PID.h @@ -0,0 +1,39 @@ +/* + * 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 + +typedef struct{ + double P, I, D; +} PIDpar_t; + +typedef struct { + PIDpar_t gain; // PID gains + double prev_error; // Previous error + double integral; // Integral term + double *pidIarray; // array for Integral + size_t pidIarrSize; // it's size + size_t curIidx; // and index of current element +} PIDController_t; + +PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz); +void pid_clear(PIDController_t *pid); +void pid_delete(PIDController_t **pid); +double pid_calculate(PIDController_t *pid, double error, double dt); diff --git a/LibSidServo/PID_test/main.c b/LibSidServo/PID_test/main.c index 5a67b97..d1b3c0f 100644 --- a/LibSidServo/PID_test/main.c +++ b/LibSidServo/PID_test/main.c @@ -22,13 +22,18 @@ #include #include "moving.h" +#include "PID.h" // errors for states: slewing/pointing/guiding -#define MAX_POINTING_ERR (50.) -#define MAX_GUIDING_ERR (5.) +// 10-degrees zone - Coordinate-driven PID +#define MAX_POINTING_ERR (36000.) +// 1-arcminute zone - Velocity-dtiven PID +#define MAX_GUIDING_ERR (60.) // timeout to "forget" old data from I sum array; seconds #define PID_I_PERIOD (3.) +// PID for coordinate-driven and velocity-driven parts +static PIDController_t *pidC = NULL, *pidV = NULL; static movemodel_t *model = NULL; static FILE *coordslog = NULL; @@ -48,93 +53,64 @@ typedef struct{ double dTcorr; double Tend; double minerr; - double P, I, D; + double startcoord; + double error; + PIDpar_t gainC, gainV; } pars; static pars G = { - .ramptype = "t", .dTmon = 0.01, .dTcorr = 0.05, .Tend = 100., .minerr = 0.1, - .P = 0.8, + .gainC.P = 0.1, + .gainV.P = 0.1, + .startcoord = 100., }; static limits_t limits = { .min = {.coord = -1e6, .speed = 0.01, .accel = 0.1}, - .max = {.coord = 1e6, .speed = 1e3, .accel = 500.}, - .jerk = 10. + .max = {.coord = 6648000, .speed = 36000., .accel = 36000.} }; -typedef struct { - double kp, ki, kd; // PID gains - double prev_error; // Previous error - double integral; // Integral term - double *pidIarray; // array for Integral - size_t pidIarrSize; // it's size - size_t curIidx; // and index of current element -} PIDController; - -static PIDController pid; static sl_option_t 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"}, {"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"}, {"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"}, {"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"}, {"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"}, {"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"}, - {"prop", NEED_ARG, NULL, 'P', arg_double, APTR(&G.P), "P-coefficient of PID"}, - {"integ", NEED_ARG, NULL, 'I', arg_double, APTR(&G.I), "I-coefficient of PID"}, - {"diff", NEED_ARG, NULL, 'D', arg_double, APTR(&G.D), "D-coefficient of PID"}, + {"propC", NEED_ARG, NULL, 'P', arg_double, APTR(&G.gainC.P), "P-coefficient of coordinate-driven PID"}, + {"integC", NEED_ARG, NULL, 'I', arg_double, APTR(&G.gainC.I), "I-coefficient of coordinate-driven PID"}, + {"diffC", NEED_ARG, NULL, 'D', arg_double, APTR(&G.gainC.D), "D-coefficient of coordinate-driven PID"}, + {"propV", NEED_ARG, NULL, 'p', arg_double, APTR(&G.gainV.P), "P-coefficient of velocity-driven PID"}, + {"integV", NEED_ARG, NULL, 'i', arg_double, APTR(&G.gainV.I), "I-coefficient of velocity-driven PID"}, + {"diffV", NEED_ARG, NULL, 'd', arg_double, APTR(&G.gainV.D), "D-coefficient of velocity-driven PID"}, + {"xstart", NEED_ARG, NULL, '0', arg_double, APTR(&G.startcoord), "starting coordinate of target"}, + {"error", NEED_ARG, NULL, 'E', arg_double, APTR(&G.error), "error range"}, // TODO: add parameters for limits setting end_option }; // calculate coordinate target for given time (starting from zero) static double target_coord(double t){ - if(t > 20. && t < 30.) return target_coord(20.); - double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5); + if(t > 20. && t < 30.) return 0.; + //double pos = G.startcoord + 15. * t + G.error * (drand48() - 0.5); + double pos = G.startcoord + 15. * sin(2*M_PI * t / 10.) + G.error * (drand48() - 0.5); return pos; } -/* P-only == oscillations -static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){ - double error = targcoord - p->coord; - if(fabs(error) < G.minerr) return p->speed; - return p->speed + error / dt / 500.; -} -*/ - -static void pid_init(PIDController *pid, double kp, double ki, double kd) { - pid->kp = fabs(kp); - pid->ki = fabs(ki); - pid->kd = fabs(kd); - pid->prev_error = 0.; - pid->integral = 0.; - pid->curIidx = 0; - pid->pidIarrSize = PID_I_PERIOD / G.dTcorr; - if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements"); - pid->pidIarray = MALLOC(double, pid->pidIarrSize); -} - -static void pid_clear(PIDController *pid){ - if(!pid) return; - bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize); - pid->integral = 0.; - pid->prev_error = 0.; - pid->curIidx = 0; -} - static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){ double error = targcoord - p->coord, fe = fabs(error); + PIDController_t *pid = NULL; switch(state){ case Slewing: if(fe < MAX_POINTING_ERR){ - pid_clear(&pid); + pid_clear(pidC); state = Pointing; green("--> Pointing\n"); + pid = pidC; }else{ red("Slewing...\n"); return (error > 0.) ? limits.max.speed : -limits.max.speed; @@ -142,54 +118,55 @@ static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){ break; case Pointing: if(fe < MAX_GUIDING_ERR){ - pid_clear(&pid); + pid_clear(pidV); state = Guiding; green("--> Guiding\n"); + pid = pidV; }else if(fe > MAX_POINTING_ERR){ red("--> Slewing\n"); state = Slewing; return (error > 0.) ? limits.max.speed : -limits.max.speed; - } + } else pid = pidC; break; case Guiding: + pid= pidV; if(fe > MAX_GUIDING_ERR){ red("--> Pointing\n"); state = Pointing; + pid_clear(pidC); + pid = pidC; }else if(fe < G.minerr){ green("At target\n"); - //pid_clear(&pid); - //return p->speed; - } + }else printf("Current error: %g\n", fe); break; } - - red("Calculate PID\n"); - double oldi = pid.pidIarray[pid.curIidx], newi = error * dt; - pid.pidIarray[pid.curIidx++] = oldi; - if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0; - pid.integral += newi - oldi; - double derivative = (error - pid.prev_error) / dt; - pid.prev_error = error; - DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative); - double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative); - if(state == Pointing) add /= 3.; - else if(state == Guiding) add /= 7.; - DBG("ADD = %g; new speed = %g", add, p->speed + add); - if(state == Guiding) return p->speed + add / dt / 10.; - return add / dt; + if(!pid){ + WARNX("where is PID?"); return p->speed; + } + double tagspeed = pid_calculate(pid, error, dt); + if(state == Guiding) return p->speed + tagspeed; + return tagspeed; } -// ./moving -l coords -P.5 -I.05 -D1.5 -// ./moving -l coords -P1.3 -D1.6 +// -P0.8 -D0.1 -I0.02 -p20 -d.5 -i.02 +// another: P0.8 -D0.1 -I0.02 -p5 -d0.9 -i0.1 static void start_model(double Tend){ - double T = 0., Tcorr = 0.;//, Tlast = 0.; + double T = 0., Tcorr = 0.; moveparam_t target; + uint64_t N = 0; + double errmax = 0., errsum = 0., errsum2 = 0.; while(T <= Tend){ moveparam_t p; movestate_t st = model->get_state(&p); if(st == ST_MOVE) st = model->proc_move(&p, T); double nextcoord = target_coord(T); double error = nextcoord - p.coord; + if(state == Guiding){ + double ae = fabs(error); + if(ae > errmax) errmax = ae; + errsum += error; errsum2 += error * error; + ++N; + } if(T - Tcorr >= G.dTcorr){ // check correction double speed = getNewSpeed(&p, nextcoord, T - Tcorr); target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5; @@ -215,6 +192,9 @@ static void start_model(double Tend){ T, nextcoord, p.coord, p.speed, p.accel, error); T += G.dTmon; } + printf("\n\n\n"); red("Calculated errors in `guiding` mode:\n"); + double mean = errsum / (double)N; + printf("max error: %g, mean error: %g, std: %g\n\n", errmax, mean, sqrt(errsum2/(double)N - mean*mean)); } int main(int argc, char **argv){ @@ -228,16 +208,15 @@ int main(int argc, char **argv){ if(G.dTmon <= 0.) ERRX("tmon should be > 0."); if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1."); if(G.Tend <= 0.) ERRX("tend should be > 0."); - pid_init(&pid, G.P, G.I, G.D); - fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error"); - 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); + pidC = pid_create(&G.gainC, PID_I_PERIOD / G.dTcorr); + pidV = pid_create(&G.gainV, PID_I_PERIOD / G.dTcorr); + if(!pidC || !pidV) ERRX("Can't init PID regulators"); + model = init_moving(&limits); if(!model) ERRX("Can't init moving model: check parameters"); + fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error"); start_model(G.Tend); + pid_delete(&pidC); + pid_delete(&pidV); fclose(coordslog); return 0; } diff --git a/LibSidServo/PID_test/moving.c b/LibSidServo/PID_test/moving.c index d7264c0..4b77040 100644 --- a/LibSidServo/PID_test/moving.c +++ b/LibSidServo/PID_test/moving.c @@ -24,12 +24,9 @@ #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; +static movemodel_t *model = &trapez; double coord_tolerance = COORD_TOLERANCE_DEFAULT; double time_tick = TIME_TICK_DEFAULT; @@ -56,21 +53,8 @@ static void chkminmax(double *min, double *max){ *max = t; } -movemodel_t *init_moving(ramptype_t type, limits_t *l){ +movemodel_t *init_moving(limits_t *l){ if(!l) return FALSE; - 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; diff --git a/LibSidServo/PID_test/moving.h b/LibSidServo/PID_test/moving.h index 6854308..eb77824 100644 --- a/LibSidServo/PID_test/moving.h +++ b/LibSidServo/PID_test/moving.h @@ -26,13 +26,6 @@ #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 @@ -48,7 +41,6 @@ typedef struct{ // all values could be both as positive and negative typedef struct{ moveparam_t min; moveparam_t max; - double jerk; } limits_t; typedef struct{ @@ -64,7 +56,7 @@ typedef struct{ extern double coord_tolerance; double nanot(); -movemodel_t *init_moving(ramptype_t type, limits_t *l); +movemodel_t *init_moving(limits_t *l); int init_coordtol(double tolerance); int init_timetick(double tick); int move_to(moveparam_t *target, double t); diff --git a/LibSidServo/PID_test/moving_model.files b/LibSidServo/PID_test/moving_model.files index e522e0e..0086d29 100644 --- a/LibSidServo/PID_test/moving_model.files +++ b/LibSidServo/PID_test/moving_model.files @@ -1,5 +1,7 @@ Dramp.c Dramp.h +PID.c +PID.h Sramp.c Sramp.h Tramp.c