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

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;