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

PID seems like almost working, but I have a great problem with model!

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-08-05 18:55:21 +03:00
parent 864e257884
commit 138e4bf84d
16 changed files with 218 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
LibSidServo/PID.c
View File

@@ -54,7 +54,7 @@ void pid_delete(PIDController_t **pid){
double pid_calculate(PIDController_t *pid, double error, double dt){
// calculate flowing integral
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
DBG("oldi/new: %g, %g", oldi, newi);
//DBG("oldi/new: %g, %g", oldi, newi);
pid->pidIarray[pid->curIidx++] = newi;
if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
pid->integral += newi - oldi;
@@ -71,68 +71,79 @@ typedef struct{
} PIDpair_t;
typedef struct{
axis_status_t *state;
axis_status_t state;
coordval_t position;
coordval_t speed;
} axisdata_t;
/**
* @brief process - Process PID for given axe
* @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
*/
static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){
if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT) return axis->speed.val; // data is too old or wrong
if(tagpos->t < axis->position.t || tagpos->t - axis->position.t > MCC_PID_MAX_DT){
DBG("target time: %g, axis time: %g - too big! (%g)", tagpos->t, axis->position.t, MCC_PID_MAX_DT);
return axis->speed.val; // data is too old or wrong
}
double error = tagpos->val - axis->position.val, fe = fabs(error);
PIDController_t *pid = NULL;
switch(*axis->state){
switch(axis->state){
case AXIS_SLEWING:
if(fe < MCC_MAX_POINTING_ERR){
*axis->state = AXIS_POINTING;
DBG("--> Pointing\n");
axis->state = AXIS_POINTING;
DBG("--> Pointing");
pid = pidpair->PIDC;
}else{
DBG("Slewing...\n");
DBG("Slewing...");
return -1.; // max speed for given axis
}
break;
case AXIS_POINTING:
if(fe < MCC_MAX_GUIDING_ERR){
*axis->state = AXIS_GUIDING;
DBG("--> Guiding\n");
axis->state = AXIS_GUIDING;
DBG("--> Guiding");
pid = pidpair->PIDV;
}else if(fe > MCC_MAX_POINTING_ERR){
DBG("--> Slewing\n");
*axis->state = AXIS_SLEWING;
DBG("--> Slewing");
axis->state = AXIS_SLEWING;
return -1.;
} else pid = pidpair->PIDC;
break;
case AXIS_GUIDING:
pid = pidpair->PIDV;
if(fe > MCC_MAX_GUIDING_ERR){
DBG("--> Pointing\n");
*axis->state = AXIS_POINTING;
DBG("--> Pointing");
axis->state = AXIS_POINTING;
pid = pidpair->PIDC;
}else if(fe < MCC_MAX_ATTARGET_ERR){
DBG("At target\n");
DBG("At target");
// TODO: we can point somehow that we are at target or introduce new axis state
}else DBG("Current error: %g\n", fe);
}else DBG("Current error: %g", fe);
break;
case AXIS_STOPPED:
case AXIS_STOPPED: // start pointing to target; will change speed next time
DBG("AXIS STOPPED!!!!");
axis->state = AXIS_SLEWING;
return -1.;
case AXIS_ERROR:
DBG("Can't move from erroneous state");
return 0.;
}
if(!pid){
DBG("WTF? Where is a PID?");
return axis->speed.val;
}
if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT) pid_clear(pid);
if(tagpos->t < pid->prevT || tagpos->t - pid->prevT > MCC_PID_MAX_DT){
DBG("time diff too big: clear PID");
pid_clear(pid);
}
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);
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; // velocity-based
return tagspeed; // coordinate-based
}
@@ -159,16 +170,24 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
mountdata_t m;
coordpair_t tagspeed;
if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
axisdata_t axe;
axe.state = &m.Xstate;
axe.position = m.encXposition;
axe.speed = m.encXspeed;
tagspeed.X = getspeed(&target->X, &pidX, &axe);
axisdata_t axis;
DBG("state: %d/%d", m.Xstate, m.Ystate);
axis.state = m.Xstate;
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;
axe.state = &m.Ystate;
axe.position = m.encYposition;
axe.speed = m.encYspeed;
tagspeed.Y = getspeed(&target->Y, &pidY, &axe);
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;
axis_status_t ystate = axis.state;
if(m.Xstate != xstate || m.Ystate != ystate){
DBG("State changed");
setStat(xstate, ystate);
}
DBG("TAG speeds: %g/%g", tagspeed.X, tagspeed.Y);
return Mount.moveWspeed(endpoint, &tagspeed);
}