eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixed bug in PID

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-08-07 17:39:13 +03:00
parent f0ab4ae770
commit 34d22614a6
11 changed files with 51 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
LibSidServo/PID.c
View File

@ -29,6 +29,7 @@ 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));
return pid;
@ -141,9 +142,9 @@ static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t
double dt = tagpos->t - pid->prevT;
if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
pid->prevT = tagpos->t;
//DBG("CALC PID (er=%g, dt=%g)", error, dt);
DBG("CALC PID (er=%g, dt=%g), state=%d", error, dt, axis->state);
double tagspeed = pid_calculate(pid, error, dt);
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed; // velocity-based
if(axis->state == AXIS_GUIDING) return axis->speed.val + tagspeed / dt; // velocity-based
return tagspeed; // coordinate-based
}
@ -176,13 +177,15 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
axis.position = m.encXposition;
axis.speed = m.encXspeed;
tagspeed.X = getspeed(&target->X, &pidX, &axis);
if(tagspeed.X < 0. || tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
if(tagspeed.X < 0.) tagspeed.X = -tagspeed.X;
if(tagspeed.X > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
axis_status_t xstate = axis.state;
axis.state = m.Ystate;
axis.position = m.encYposition;
axis.speed = m.encYspeed;
tagspeed.Y = getspeed(&target->Y, &pidY, &axis);
if(tagspeed.Y < 0. || tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
if(tagspeed.Y < 0.) tagspeed.Y = -tagspeed.Y;
if(tagspeed.Y > MCC_MAX_Y_SPEED) tagspeed.Y = MCC_MAX_Y_SPEED;
axis_status_t ystate = axis.state;
if(m.Xstate != xstate || m.Ystate != ystate){
DBG("State changed");

6
LibSidServo/examples/dump.c
View File

@ -67,17 +67,13 @@ static double filter(double val, int idx){
void logmnt(FILE *fcoords, mountdata_t *m){
if(!fcoords) return;
//DBG("LOG %s", m ? "data" : "header");
static double t0 = -1.;
if(!m){ // write header
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
return;
}
double tnow = (m->encXposition.t + m->encYposition.t) / 2.;
if(t0 < 0.) t0 = tnow;
double t = tnow - t0;
// write data
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
m->motXposition.t, RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
m->millis);

6
LibSidServo/examples/scmd_traectory.c
View File

@ -107,7 +107,7 @@ static void runtraectory(traectory_fn tfn){
return;
}
if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind
DBG("\n\nTELPOS: %g'/%g' measured @ %g/%g", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), telXY.X.t, telXY.Y.t);
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f) measured @ %.6f/%.6f", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val), telXY.X.t, telXY.Y.t);
tlastX = telXY.X.t; tlastY = telXY.Y.t;
double t = Mount.currentT();
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break;
@ -119,9 +119,9 @@ static void runtraectory(traectory_fn tfn){
else if(telXY.X.val < traectXY.X) endpoint.X = G.Xmax;
if(telXY.Y.val > traectXY.Y) endpoint.Y = -G.Ymax;
else if(telXY.Y.val < traectXY.Y) endpoint.Y = G.Ymax;
DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
DBG("%g: dX=%.4f'', dY=%.4f''", t-t0, 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));
//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.4g %10.4g %10.4g\n", t, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
if(MCC_E_OK != Mount.correctTo(&target, &endpoint)) WARNX("Error of correction!");

1
LibSidServo/examples/traectories.c
View File

@ -76,6 +76,7 @@ int telpos(coordval_pair_t *curpos){
}
if(ntries == 10) return FALSE;
coordval_pair_t pt;
//DBG("\n\nTELPOS: %g'/%g' measured @ %.6f", RAD2AMIN(mdata.encXposition.val), RAD2AMIN(mdata.encYposition.val), mdata.encXposition.t);
pt.X.val = mdata.encXposition.val;
pt.Y.val = mdata.encYposition.val;
pt.X.t = mdata.encXposition.t;

17
LibSidServo/main.c
View File

@ -75,10 +75,8 @@ static void quit(){
DBG("Exit");
}
void getModData(mountdata_t *mountdata){
if(!mountdata || !Xmodel || !Ymodel) return;
static double oldmt = -100.; // old `millis measurement` time
static uint32_t oldmillis = 0;
void getModData(coordval_pair_t *c){
if(!c || !Xmodel || !Ymodel) return;
double tnow = nanotime();
moveparam_t Xp, Yp;
movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
@ -86,14 +84,9 @@ void getModData(mountdata_t *mountdata){
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow;
mountdata->motXposition.val = mountdata->encXposition.val = Xp.coord;
mountdata->motYposition.val = mountdata->encYposition.val = Yp.coord;
getXspeed(); getYspeed();
if(tnow - oldmt > Conf.MountReqInterval){
oldmillis = mountdata->millis = (uint32_t)(tnow * 1e3);
oldmt = tnow;
}else mountdata->millis = oldmillis;
c->X.t = c->Y.t = tnow;
c->X.val = Xp.coord;
c->Y.val = Yp.coord;
}
/**

2
LibSidServo/main.h
View File

@ -28,7 +28,7 @@
extern conf_t Conf;
double nanotime();
void getModData(mountdata_t *mountdata);
void getModData(coordval_pair_t *c);
typedef struct{
double *x, *t, *t2, *xt; // arrays of coord/time and multiply
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply

2
LibSidServo/movingmodel.c
View File

@ -60,7 +60,7 @@ movemodel_t *model_init(limits_t *l){
int model_move2(movemodel_t *model, moveparam_t *target, double t){
if(!target || !model) return FALSE;
DBG("MOVE to %g at speed %g", target->coord, target->speed);
//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

9
LibSidServo/ramp.c
View File

@ -120,7 +120,7 @@ static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
}
}else{
// if we are here, we have the worst case: change speed direction
DBG("Hardest case: change speed direction");
// DBG("Hardest case: change speed direction");
// now we should calculate coordinate at which model stops and biuld new trapezium from that point
double x0 = m->curparams.coord, v0 = m->curparams.speed;
double xstop = x0 + sign_v0 * abs_dx_stop, tstop = t + abs_v0 / abs_a;
@ -132,7 +132,7 @@ static void unlockedcalc(movemodel_t *m, moveparam_t *x, double t){
m->Times[STAGE_ACCEL] = t;
m->Params[STAGE_ACCEL].coord = x0;
m->Params[STAGE_ACCEL].speed = v0;
DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
// DBG("NOW t[0]=%g, X[0]=%g, V[0]=%g", t, x0, v0);
return;
}
m->state = ST_MOVE;
@ -210,11 +210,10 @@ static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
if(m->movingstage == STAGE_STOPPED){
m->curparams.coord = m->Params[STAGE_STOPPED].coord;
pthread_mutex_unlock(&m->mutex);
DBG("REACHED STOPping stage @ t=%g", t);
/* DBG("REACHED STOPping stage @ t=%g", t);
for(int s = STAGE_STOPPED; s >= 0; --s){
DBG("T[%d]=%g, ", s, m->Times[s]);
}
fflush(stdout);
}*/
emstop(m, t);
goto ret;
}

27
LibSidServo/serial.c
View File

@ -64,11 +64,13 @@ void getXspeed(){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
pthread_mutex_lock(&datamutex);
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t);
if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
mountdata.encXspeed.val = speed;
mountdata.encXspeed.t = mountdata.encXposition.t;
}
pthread_mutex_unlock(&datamutex);
//DBG("Xspeed=%g", mountdata.encXspeed.val);
#if 0
mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
@ -83,11 +85,13 @@ void getYspeed(){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
pthread_mutex_lock(&datamutex);
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t);
if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
mountdata.encYspeed.val = speed;
mountdata.encYspeed.t = mountdata.encYposition.t;
}
pthread_mutex_unlock(&datamutex);
#if 0
mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.;
@ -325,15 +329,19 @@ static void *encoderthread2(void _U_ *u){
}
double v, t;
if(getencval(encfd[0], &v, &t)){
pthread_mutex_lock(&datamutex);
mountdata.encXposition.val = X_ENC2RAD(v);
//DBG("encX(%g) = %g", t, mountdata.encXposition.val);
mountdata.encXposition.t = t;
pthread_mutex_unlock(&datamutex);
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
getXspeed();
if(getencval(encfd[1], &v, &t)){
pthread_mutex_lock(&datamutex);
mountdata.encYposition.val = Y_ENC2RAD(v);
//DBG("encY(%g) = %g", t, mountdata.encYposition.val);
mountdata.encYposition.t = t;
pthread_mutex_unlock(&datamutex);
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
getYspeed();
errctr = 0;
@ -384,12 +392,25 @@ static void *mountthread(void _U_ *u){
uint8_t buf[2*sizeof(SSstat)];
SSstat *status = (SSstat*) buf;
bzero(&mountdata, sizeof(mountdata));
double t0 = nanotime();
static double oldmt = -100.; // old `millis measurement` time
static uint32_t oldmillis = 0;
if(Conf.RunModel) while(1){
pthread_mutex_lock(&datamutex);
coordval_pair_t c;
// now change data
getModData(&mountdata);
getModData(&c);
pthread_mutex_lock(&datamutex);
double tnow = c.X.t;
mountdata.motXposition.t = mountdata.encXposition.t = mountdata.motYposition.t = mountdata.encYposition.t = tnow;
mountdata.motXposition.val = mountdata.encXposition.val = c.X.val;
mountdata.motYposition.val = mountdata.encYposition.val = c.Y.val;
//DBG("t=%g, X=%g, Y=%g", tnow, c.X.val, c.Y.val);
if(tnow - oldmt > Conf.MountReqInterval){
oldmillis = mountdata.millis = (uint32_t)(tnow * 1e3);
oldmt = tnow;
}else mountdata.millis = oldmillis;
pthread_mutex_unlock(&datamutex);
double t0 = nanotime();
getXspeed(); getYspeed();
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
t0 = nanotime();
}

5
LibSidServo/sidservo.h
View File

@ -56,9 +56,10 @@ extern "C"
// normal PID refresh interval
#define MCC_PID_REFRESH_DT (0.1)
// boundary conditions for axis state: "slewing/pointing/guiding"
// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 5 degrees
// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 8 degrees
//#define MCC_MAX_POINTING_ERR (0.20943951)
#define MCC_MAX_POINTING_ERR (0.08726646)
//#define MCC_MAX_POINTING_ERR (0.08726646)
#define MCC_MAX_POINTING_ERR (0.13962634)
// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "guiding": 1.5 deg
#define MCC_MAX_GUIDING_ERR (0.026179939)
// if error less than this value we suppose that target is captured and guiding is good: 0.1''

1
LibSidServo/ssii.c
View File

@ -18,6 +18,7 @@
#include <ctype.h>
#include <inttypes.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>