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
Download Patch File Download Diff File Expand all files Collapse all files

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;
}