From db1058ddf8a1738987fbdd0c4bb676bf646a9286 Mon Sep 17 00:00:00 2001 From: "Edward V. Emelianov" Date: Wed, 18 Mar 2026 18:10:05 +0300 Subject: [PATCH] fixed broken PID --- .gitignore | 90 +++++++++++++++++++++++++++++ PID.c | 116 +++++++++++++++++++++----------------- PID.h | 13 +---- examples/scmd_traectory.c | 27 ++++++--- examples/traectories.c | 22 ++++++-- serial.c | 12 +++- sidservo.h | 8 +-- 7 files changed, 204 insertions(+), 84 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0c4cda4 --- /dev/null +++ b/.gitignore @@ -0,0 +1,90 @@ +# ---> C +# Prerequisites +*.d + +# Object files +*.o +*.ko +*.obj +*.elf + +# Linker output +*.ilk +*.map +*.exp + +# Precompiled Headers +*.gch +*.pch + +# Libraries +*.lib +*.a +*.la +*.lo + +# Shared objects (inc. Windows DLLs) +*.dll +*.so +*.so.* +*.dylib + +# Executables +*.exe +*.out +*.app +*.i*86 +*.x86_64 +*.hex + +# Debug files +*.dSYM/ +*.su +*.idb +*.pdb + +# Kernel Module Compile Results +*.mod* +*.cmd +.tmp_versions/ +modules.order +Module.symvers +Mkfile.old +dkms.conf + +# ---> C++ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +build +CMakeLists.txt.user* diff --git a/PID.c b/PID.c index 3f642cd..0178c3e 100644 --- a/PID.c +++ b/PID.c @@ -16,6 +16,7 @@ * along with this program. If not, see . */ +#include #include #include #include @@ -25,34 +26,70 @@ #include "PID.h" #include "serial.h" -PIDController_t *pid_create(const PIDpar_t *gain, size_t Iarrsz){ +typedef struct { + PIDpar_t gain; // PID gains + double prev_error; // Previous error + double prev_tagpos; // previous target position + double integral; // Integral term + double *pidIarray; // array for Integral + struct timespec prevT; // time of previous correction + size_t pidIarrSize; // it's size + size_t curIidx; // and index of current element +} PIDController_t; + +typedef struct{ + axis_status_t state; + coordval_t position; + coordval_t speed; +} axisdata_t; + +static PIDController_t *pid_create(const 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; DBG("Created PID with P=%g, I=%g, D=%g\n", gain->P, gain->I, gain->D); pid->pidIarrSize = Iarrsz; pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double)); + curtime(&pid->prevT); return pid; } // don't clear lastT! -void pid_clear(PIDController_t *pid){ +static 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; + curtime(&pid->prevT); } - -void pid_delete(PIDController_t **pid){ +/* +static 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 new motor speed +static double pid_calculate(PIDController_t *pid, double axispos, const coordval_t *target){ + double dtpid = timediff(&target->t, &pid->prevT); + if(dtpid < 0 || dtpid > Conf.PIDMaxDt){ + DBG("time diff too big: clear PID"); + pid_clear(pid); + pid->prev_tagpos = target->val; + return 0.; + } + double dt = timediff(&target->t, &pid->prevT); + if(dt < FLT_EPSILON){ + DBG("Target time in past"); + return 0.; + } + pid->prevT = target->t; + double error = target->val - axispos; + double tagspeed = (target->val - pid->prev_tagpos) / dt; + pid->prev_tagpos = target->val; // calculate flowing integral double oldi = pid->pidIarray[pid->curIidx], newi = error * dt; //DBG("oldi/new: %g, %g", oldi, newi); @@ -61,43 +98,33 @@ double pid_calculate(PIDController_t *pid, double error, double dt){ 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); + DBG("pid pars: P=%g, I=%g, D=%f", pid->gain.P, pid->gain.I, pid->gain.D); + double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative + tagspeed; + DBG("tagspeed=%g, P=%g, I=%g, D=%g; sum=%g", tagspeed, pid->gain.P * error, + pid->gain.I * pid->integral, pid->gain.D * derivative, sum); return sum; } -typedef struct{ - PIDController_t *PIDC; - PIDController_t *PIDV; -} PIDpair_t; - -typedef struct{ - axis_status_t state; - coordval_t position; - coordval_t speed; -} axisdata_t; /** * @brief process - Process PID for given axis * @param tagpos - given coordinate of target position * @param endpoint - endpoint for this coordinate * @param pid - pid itself - * @return calculated new speed or -1 for max speed + * @return calculated NEW SPEED or NAN for max speed */ -static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){ +static double getspeed(const coordval_t *tagpos, PIDController_t *pid, axisdata_t *axis){ double dt = timediff(&tagpos->t, &axis->position.t); if(dt < 0 || dt > Conf.PIDMaxDt){ DBG("target time: %ld, axis time: %ld - too big! (tag-ax=%g)", tagpos->t.tv_sec, axis->position.t.tv_sec, dt); return axis->speed.val; // data is too old or wrong } double error = tagpos->val - axis->position.val, fe = fabs(error); - DBG("error: %g", error); - PIDController_t *pid = NULL; + DBG("error: %g'', cur speed: %g (deg/s)", error * 180. * 3600. / M_PI, axis->speed.val*180./M_PI); switch(axis->state){ case AXIS_SLEWING: if(fe < Conf.MaxPointingErr){ axis->state = AXIS_POINTING; DBG("--> Pointing"); - pid = pidpair->PIDC; }else{ DBG("Slewing..."); return NAN; // max speed for given axis @@ -107,28 +134,25 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t if(fe < Conf.MaxFinePointingErr){ axis->state = AXIS_GUIDING; DBG("--> Guiding"); - pid = pidpair->PIDV; }else if(fe > Conf.MaxPointingErr){ DBG("--> Slewing"); axis->state = AXIS_SLEWING; return NAN; - } else pid = pidpair->PIDC; + } break; case AXIS_GUIDING: - pid = pidpair->PIDV; if(fe > Conf.MaxFinePointingErr){ DBG("--> Pointing"); axis->state = AXIS_POINTING; - pid = pidpair->PIDC; }else if(fe < Conf.MaxGuidingErr){ DBG("At target"); // TODO: we can point somehow that we are at target or introduce new axis state - }else DBG("Current error: %g", fe); + }else DBG("Current abs error: %g", fe); break; case AXIS_STOPPED: // start pointing to target; will change speed next time DBG("AXIS STOPPED!!!! --> Slewing"); axis->state = AXIS_SLEWING; - return getspeed(tagpos, pidpair, axis); + return getspeed(tagpos, pid, axis); case AXIS_ERROR: DBG("Can't move from erroneous state"); return 0.; @@ -137,17 +161,7 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t DBG("WTF? Where is a PID?"); return axis->speed.val; } - double dtpid = timediff(&tagpos->t, &pid->prevT); - if(dtpid < 0 || dtpid > Conf.PIDMaxDt){ - DBG("time diff too big: clear PID"); - pid_clear(pid); - } - if(dtpid > Conf.PIDMaxDt) dtpid = Conf.PIDCycleDt; - pid->prevT = tagpos->t; - DBG("CALC PID (er=%g, dt=%g), state=%d", error, dtpid, axis->state); - double tagspeed = pid_calculate(pid, error, dtpid); - if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dtpid; // velocity-based - return tagspeed; // coordinate-based + return pid_calculate(pid, axis->position.val, tagpos); } /** @@ -157,18 +171,14 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t * @return error code */ mcc_errcodes_t correct2(const coordval_pair_t *target){ - static PIDpair_t pidX = {0}, pidY = {0}; - if(!pidX.PIDC){ - pidX.PIDC = pid_create(&Conf.XPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); - if(!pidX.PIDC) return MCC_E_FATAL; - pidX.PIDV = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); - if(!pidX.PIDV) return MCC_E_FATAL; + static PIDController_t *pidX = NULL, *pidY = NULL; + if(!pidX){ + pidX = pid_create(&Conf.XPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); + if(!pidX) return MCC_E_FATAL; } - if(!pidY.PIDC){ - pidY.PIDC = pid_create(&Conf.YPIDC, Conf.PIDCycleDt / Conf.PIDRefreshDt); - if(!pidY.PIDC) return MCC_E_FATAL; - pidY.PIDV = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); - if(!pidY.PIDV) return MCC_E_FATAL; + if(!pidY){ + pidY = pid_create(&Conf.YPIDV, Conf.PIDCycleDt / Conf.PIDRefreshDt); + if(!pidY) return MCC_E_FATAL; } mountdata_t m; coordpair_t tagspeed; // absolute value of speed @@ -179,7 +189,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){ axis.state = m.Xstate; axis.position = m.encXposition; axis.speed = m.encXspeed; - tagspeed.X = getspeed(&target->X, &pidX, &axis); + tagspeed.X = getspeed(&target->X, pidX, &axis); if(isnan(tagspeed.X)){ // max speed if(target->X.val < axis.position.val) Xsign = -1.; tagspeed.X = Xlimits.max.speed; @@ -191,7 +201,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){ axis.state = m.Ystate; axis.position = m.encYposition; axis.speed = m.encYspeed; - tagspeed.Y = getspeed(&target->Y, &pidY, &axis); + tagspeed.Y = getspeed(&target->Y, pidY, &axis); if(isnan(tagspeed.Y)){ // max speed if(target->Y.val < axis.position.val) Ysign = -1.; tagspeed.Y = Ylimits.max.speed; diff --git a/PID.h b/PID.h index 5ecfad0..79539f7 100644 --- a/PID.h +++ b/PID.h @@ -22,19 +22,10 @@ #include "sidservo.h" -typedef struct { - PIDpar_t gain; // PID gains - double prev_error; // Previous error - double integral; // Integral term - double *pidIarray; // array for Integral - struct timespec prevT; // time of previous correction - size_t pidIarrSize; // it's size - size_t curIidx; // and index of current element -} PIDController_t; - +/* PIDController_t *pid_create(const 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); - +*/ mcc_errcodes_t correct2(const coordval_pair_t *target); diff --git a/examples/scmd_traectory.c b/examples/scmd_traectory.c index 91af985..71981d2 100644 --- a/examples/scmd_traectory.c +++ b/examples/scmd_traectory.c @@ -109,17 +109,27 @@ static void runtraectory(traectory_fn tfn){ } if(!Mount.currentT(&tcur)) continue; if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind - DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val)); tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec; double t = Mount.timeFromStart(); - if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break; - if(!traectory_point(&traectXY, t)) break; + if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax){ + if(fabs(telXY.X.val) > G.Xmax) DBG("X over maximal limit!"); + if(fabs(telXY.Y.val) > G.Ymax) DBG("Y over maximal limit!"); + if(t - tstart > G.tmax) DBG("Time over..."); + break; + } + if(!traectory_point(&traectXY, t)){ + DBG("Error in 'traectory_point', time from start=%g", t); + break; + } + DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f); traectory: %g'/%g' (%.6f/%.6f)", + RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), + RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y), RAD2DEG(traectXY.X), RAD2DEG(traectXY.Y)); target.X.val = traectXY.X; target.Y.val = traectXY.Y; target.X.t = target.Y.t = tcur; if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0); else{ //DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y)); - DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); + DBG("%g, target-telescope: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); //DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y)); if(errlog) fprintf(errlog, "%10.4f %10.4f %10.4f\n", Mount.timeDiff(&telXY.X.t, &t0), RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val)); @@ -166,15 +176,16 @@ int main(int argc, char **argv){ return 1; } coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)}; - if(!init_traectory(tfn, &c)){ - ERRX("Can't init traectory"); - return 1; - } mcc_errcodes_t e = Mount.init(Config); if(e != MCC_E_OK){ WARNX("Can't init devices"); return 1; } + // run this function only after mount inited! + if(!init_traectory(tfn, &c)){ + ERRX("Can't init traectory"); + return 1; + } signal(SIGTERM, signals); // kill (-15) - quit signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGINT, signals); // ctrl+C - quit diff --git a/examples/traectories.c b/examples/traectories.c index d8464af..3f26a5a 100644 --- a/examples/traectories.c +++ b/examples/traectories.c @@ -42,6 +42,7 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){ cur_traectory = f; XYstart = *XY0; tstart = Mount.timeFromStart(); + DBG("inited @ %gs from start", tstart); mountdata_t mdata; int ntries = 0; for(; ntries < 10; ++ntries){ @@ -58,11 +59,21 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){ * @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180) */ int traectory_point(coordpair_t *nextpt, double t){ - if(t < 0. || !cur_traectory) return FALSE; + if(t < 0. || !cur_traectory){ + if(t < 0.) DBG("time in past!"); + else DBG("no current traectory selected!"); + return FALSE; + } coordpair_t pt; - if(!cur_traectory(&pt, t)) return FALSE; + if(!cur_traectory(&pt, t)){ + DBG("error in cur_traectory"); + return FALSE; + } if(nextpt) *nextpt = pt; - if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE; + if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI){ + DBG("some points is over limits, pt.x=%g, pt.y=%g degrees", RAD2DEG(pt.X), RAD2DEG(pt.Y)); + return FALSE; + } return TRUE; } @@ -88,6 +99,7 @@ int telpos(coordval_pair_t *curpos){ // X=X0+1'/s, Y=Y0+15''/s int Linear(coordpair_t *nextpt, double t){ coordpair_t pt; + DBG("t=%g, tfromstart=%g, dt=%g", t, tstart, t-tstart); pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart); pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart); if(nextpt) *nextpt = pt; @@ -98,7 +110,7 @@ int Linear(coordpair_t *nextpt, double t){ int SinCos(coordpair_t *nextpt, double t){ coordpair_t pt; pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI); - pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/200.*2*M_PI); + pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/10.*2*M_PI); if(nextpt) *nextpt = pt; return TRUE; } @@ -111,7 +123,7 @@ typedef struct{ static tr_names names[] = { {Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"}, - {SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"}, + {SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+1'*cos(t/10*2pi)"}, {NULL, NULL, NULL} }; diff --git a/serial.c b/serial.c index 25b0ab9..67e06b7 100644 --- a/serial.c +++ b/serial.c @@ -382,7 +382,10 @@ static int readstrings(buf_t *buf, int fd){ // return TRUE if got, FALSE if no data found static int getdata(buf_t *buf, long *out){ - if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)) return FALSE; + if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)){ + return FALSE; + } + DBG("got data"); // read record between last '\n' and previous (or start of string) char *last = &buf->buf[buf->len - 1]; //DBG("buf: _%s_", buf->buf); @@ -439,6 +442,7 @@ static void *encoderthread2(void _U_ *u){ for(int i = 0; i < 2; ++i){ if(pfds[i].revents && POLLIN){ if(!readstrings(&strbuf[i], encfd[i])){ + DBG("ERR"); ++errctr; break; } @@ -446,7 +450,9 @@ static void *encoderthread2(void _U_ *u){ double curt = timefromstart(); if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt; if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records + DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]); if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){ + DBG("time OK"); pthread_mutex_lock(&datamutex); double pos = (double)msrlast[i]; //DBG("pos[%d]=%g", i, pos); @@ -626,8 +632,8 @@ static int ttyopen(const char *path, speed_t speed){ tty.c_cflag = BOTHER | CS8 | CREAD | CLOCAL; // other speed, 8bit, RW, ignore line ctrl tty.c_ispeed = speed; tty.c_ospeed = speed; - //tty.c_cc[VMIN] = 0; // non-canonical mode - //tty.c_cc[VTIME] = 5; + tty.c_cc[VMIN] = 0; // non-canonical mode + tty.c_cc[VTIME] = 5; if(ioctl(fd, TCSETS2, &tty)){ DBG("Can't set TTY settings"); close(fd); diff --git a/sidservo.h b/sidservo.h index ed4feab..e544ac1 100644 --- a/sidservo.h +++ b/sidservo.h @@ -136,10 +136,10 @@ typedef struct{ } extradata_t; typedef enum{ - AXIS_STOPPED, - AXIS_SLEWING, - AXIS_POINTING, - AXIS_GUIDING, + AXIS_STOPPED, // stop + AXIS_SLEWING, // go to target with maximal speed + AXIS_POINTING, // axis is in pointing zone, use PID + AXIS_GUIDING, // near target AXIS_ERROR, } axis_status_t;