moving model

LibSidServo/PID_test/main.c

@@ -18,21 +18,27 @@
 
 #include <math.h>
 #include <stdio.h>
+#include <strings.h>
 #include <usefull_macros.h>
 
 #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){
+    switch(state){
-        if(fe < MAX_GUIDING_ERR) state = Guiding;
+        case Slewing:
-        return (error > 0.) ? limits.max.speed : -limits.max.speed;
+            if(fe < MAX_POINTING_ERR){
-    }else if(fe < G.minerr) return p->speed;
+                pid_clear(&pid);
-    pid.integral += error * dt;
+                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);
+    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) / dt  / 1000.;
+    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");

LibSidServo/PID_test/moving_model.creator.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 15.0.1, 2025-04-14T21:38:19. -->
+<!-- Written by QtCreator 17.0.0, 2025-07-29T13:32:31. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -13,8 +13,8 @@
  <data>
   <variable>ProjectExplorer.Project.EditorSettings</variable>
   <valuemap type="QVariantMap">
+   <value type="bool" key="EditorConfiguration.AutoDetect">true</value>
    <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
-   <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
    <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
    <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
     <value type="QString" key="language">Cpp</value>
@@ -92,6 +92,7 @@
   <variable>ProjectExplorer.Project.Target.0</variable>
   <valuemap type="QVariantMap">
    <value type="QString" key="DeviceType">Desktop</value>
+   <value type="bool" key="HasPerBcDcs">true</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
@@ -133,6 +134,41 @@
     <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
+    <value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
+    <value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+      <value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
+     <valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
+     <value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
+    </valuemap>
+    <value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+     <value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
+     <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
+     <value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
+     <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+     <value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
+     <value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
+     <valuelist type="QVariantList" key="CustomOutputParsers"/>
+     <value type="int" key="PE.EnvironmentAspect.Base">2</value>
+     <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
+     <value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
+     <value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
+     <value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
+     <value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
+     <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+     <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
+    </valuemap>
+    <value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
    </valuemap>
    <value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">

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

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