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