From 89b86f8b7f7694e39a6f8c2d74f5aeb2f7f1d460 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Tue, 29 Jul 2025 13:34:17 +0300 Subject: [PATCH] moving model --- LibSidServo/PID_test/main.c | 83 ++++++++++++++++--- .../PID_test/moving_model.creator.user | 40 ++++++++- LibSidServo/PID_test/plot_jpg | 6 ++ LibSidServo/PID_test/ploterr_jpg | 5 ++ 4 files changed, 120 insertions(+), 14 deletions(-) create mode 100755 LibSidServo/PID_test/plot_jpg create mode 100755 LibSidServo/PID_test/ploterr_jpg diff --git a/LibSidServo/PID_test/main.c b/LibSidServo/PID_test/main.c index ad9647d..5a67b97 100644 --- a/LibSidServo/PID_test/main.c +++ b/LibSidServo/PID_test/main.c @@ -18,21 +18,27 @@ #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 = Pointing; +static state_t state = Slewing; typedef struct{ int help; @@ -64,6 +70,9 @@ 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; @@ -86,7 +95,7 @@ static sl_option_t opts[] = { // 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 = 5. + 10. * sin(M_2_PI * t / 10.) + 0.2 * (drand48() - 0.5); + double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5); return pos; } @@ -104,23 +113,73 @@ static void pid_init(PIDController *pid, double kp, double ki, double kd) { 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); - if(state == Pointing){ - if(fe < MAX_GUIDING_ERR) state = Guiding; - return (error > 0.) ? limits.max.speed : -limits.max.speed; - }else if(fe < G.minerr) return p->speed; - pid.integral += error * dt; + 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("I=%g", pid.integral); - double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative) / dt / 1000.; + 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); - return 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.; @@ -167,7 +226,7 @@ int main(int argc, char **argv){ 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.) ERRX("tcor 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"); diff --git a/LibSidServo/PID_test/moving_model.creator.user b/LibSidServo/PID_test/moving_model.creator.user index 2c3598d..eaf30ec 100644 --- a/LibSidServo/PID_test/moving_model.creator.user +++ b/LibSidServo/PID_test/moving_model.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -13,8 +13,8 @@ ProjectExplorer.Project.EditorSettings + true true - false true Cpp @@ -92,6 +92,7 @@ ProjectExplorer.Project.Target.0 Desktop + true Desktop Desktop {91347f2c-5221-46a7-80b1-0a054ca02f79} @@ -133,6 +134,41 @@ По умолчанию 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 diff --git a/LibSidServo/PID_test/plot_jpg b/LibSidServo/PID_test/plot_jpg new file mode 100755 index 0000000..f926de6 --- /dev/null +++ b/LibSidServo/PID_test/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/ploterr_jpg b/LibSidServo/PID_test/ploterr_jpg new file mode 100755 index 0000000..b1bd63b --- /dev/null +++ b/LibSidServo/PID_test/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