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

16
LibSidServo/examples/SSIIconf.c
View File

@@ -51,10 +51,10 @@ static sl_option_t confopts[] = {
end_option
};
static void dumpaxe(char axe, axe_config_t *c){
static void dumpaxis(char axis, axis_config_t *c){
#define STRUCTPAR(p) (c)->p
#define DUMP(par) do{printf("%c%s=%g\n", axe, #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%c%s=%g\n", axe, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
#define DUMP(par) do{printf("%c%s=%g\n", axis, #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
DUMPD(accel);
DUMPD(backlash);
DUMPD(errlimit);
@@ -99,12 +99,12 @@ static void dumpHWconf(){
#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
green("X axe configuration:\n");
dumpaxe('X', &HW.Xconf);
green("X axis configuration:\n");
dumpaxis('X', &HW.Xconf);
green("X bits:\n");
dumpxbits(&HW.xbits);
green("Y axe configuration:\n");
dumpaxe('Y', &HW.Yconf);
green("Y axis configuration:\n");
dumpaxis('Y', &HW.Yconf);
green("Y bits:\n");
dumpybits(&HW.ybits);
green("Other:\n");
@@ -143,10 +143,12 @@ int main(int argc, char** argv){
}
if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
/*
char *c = sl_print_opts(confopts, TRUE);
green("Got configuration:\n");
printf("%s\n", c);
FREE(c);
*/
dumpHWconf();
/*
if(G.hwconffile && G.writeconf){

18
LibSidServo/examples/conf.c
View File

@@ -31,14 +31,18 @@ static conf_t Config = {
.EncoderReqInterval = 0.05,
.SepEncoder = 2,
.EncoderSpeedInterval = 0.1,
.XPIDC.P = 0.5,
.XPIDC.D = 0.05,
.XPIDV.P = 0.5,
.XPIDV.D = 0.05,
.YPIDC.P = 0.5,
.YPIDC.D = 0.05,
.XPIDC.P = 0.8,
.XPIDC.I = 0.1,
.XPIDC.D = 0.3,
.XPIDV.P = 1.,
.XPIDV.I = 0.01,
.XPIDV.D = 0.2,
.YPIDC.P = 0.8,
.YPIDC.I = 0.1,
.YPIDC.D = 0.3,
.YPIDV.P = 0.5,
.YPIDV.D = 0.05,
.YPIDV.I = 0.2,
.YPIDV.D = 0.5,
};
static sl_option_t opts[] = {

2
LibSidServo/examples/dump.c
View File

@@ -127,7 +127,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
}
/**
* @brief waitmoving - wait until moving by both axes stops at least for N cycles
* @brief waitmoving - wait until moving by both axiss stops at least for N cycles
* @param N - amount of stopped cycles
*/
void waitmoving(int N){

49
LibSidServo/examples/scmd_traectory.c
View File

@@ -38,11 +38,12 @@ typedef struct{
double X0; // starting point of traectory (-30..30 degr)
double Y0; // -//-
char *coordsoutput; // dump file
char *errlog; // log with position errors
char *tfn; // traectory function name
char *conffile;
} parameters;
static FILE *fcoords = NULL;
static FILE *fcoords = NULL, *errlog = NULL;
static pthread_t dthr;
static parameters G = {
.Ncycles = 40,
@@ -61,12 +62,13 @@ static sl_option_t cmdlnopts[] = {
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1 second)"},
{"traectory", NEED_ARG, NULL, 't', arg_string, APTR(&G.tfn), "used traectory function (default: sincos)"},
{"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal X coordinate for traectory (default: 45 degrees)"},
{"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal X coordinate for traectory (default: 45 degrees)"},
{"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal abs X coordinate for traectory (default: 45 degrees)"},
{"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal abs Y coordinate for traectory (default: 45 degrees)"},
{"tmax", NEED_ARG, NULL, 'T', arg_double, APTR(&G.tmax), "maximal duration time of emulation (default: 300 seconds)"},
{"x0", NEED_ARG, NULL, '0', arg_double, APTR(&G.X0), "starting X-coordinate of traectory (default: 10 degrees)"},
{"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"},
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
{"errlog", NEED_ARG, NULL, 'e', arg_string, APTR(&G.errlog), "file with errors log"},
end_option
};
@@ -76,6 +78,8 @@ void signals(int sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.stop();
sleep(1);
Mount.quit();
if(fcoords) fclose(fcoords);
exit(sig);
@@ -90,22 +94,39 @@ static void *dumping(void _U_ *u){
static void runtraectory(traectory_fn tfn){
if(!tfn) return;
coordval_pair_t telXY;
coordpair_t traectXY;
double t0 = Mount.currentT();
double tlast = 0.;
coordval_pair_t target;
coordpair_t traectXY, endpoint;
endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
double t0 = Mount.currentT(), tlast = 0.;
double tlastX = 0., tlastY = 0.;
while(1){
if(!telpos(&telXY)){
WARNX("No next telescope position");
return;
}
if(telXY.X.t == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind
tlast = (telXY.X.t + telXY.Y.t) / 2.;
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);
tlastX = telXY.X.t; tlastY = telXY.Y.t;
double t = Mount.currentT();
if(telXY.X.val > G.Xmax || telXY.Y.val > G.Ymax || t - t0 > G.tmax) break;
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - t0 > G.tmax) break;
if(!traectory_point(&traectXY, t)) break;
DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
target.X.val = traectXY.X; target.Y.val = traectXY.Y;
target.X.t = target.Y.t = t;
// check whether we should change direction
if(telXY.X.val > traectXY.X) endpoint.X = -G.Xmax;
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("%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));
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!");
while((t = Mount.currentT()) - tlast < MCC_PID_REFRESH_DT) usleep(50);
tlast = t;
}
WARNX("No next traectory point");
WARNX("No next traectory point or emulation ends");
}
int main(int argc, char **argv){
@@ -118,6 +139,12 @@ int main(int argc, char **argv){
G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
ERRX("X0 and Y0 should be -30..30 degrees");
if(G.errlog){
if(!(errlog = fopen(G.errlog, "w")))
ERRX("Can't open error log %s", G.errlog);
else
fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n");
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);

12
LibSidServo/examples/simpleconv.h
View File

@@ -20,10 +20,10 @@
#include <math.h>
#define DEG2RAD(d) (d/180.*M_PI)
#define ASEC2RAD(d) (d/180.*M_PI/3600.)
#define AMIN2RAD(d) (d/180.*M_PI/60.)
#define RAD2DEG(r) (r/M_PI*180.)
#define RAD2ASEC(r) (r/M_PI*180.*3600.)
#define RAD2AMIN(r) (r/M_PI*180.*60.)
#define DEG2RAD(d) ((d)/180.*M_PI)
#define ASEC2RAD(d) ((d)/180.*M_PI/3600.)
#define AMIN2RAD(d) ((d)/180.*M_PI/60.)
#define RAD2DEG(r) ((r)/M_PI*180.)
#define RAD2ASEC(r) ((r)/M_PI*180.*3600.)
#define RAD2AMIN(r) ((r)/M_PI*180.*60.)

19
LibSidServo/examples/traectories.c
View File

@@ -30,10 +30,6 @@ static traectory_fn cur_traectory = NULL;
// starting point of traectory
static coordpair_t XYstart = {0};
static double tstart = 0.;
// convert Xe/Ye to approximate motor coordinates:
// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have
// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0
static coordval_pair_t XYcor = {0};
/**
* @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
@@ -52,9 +48,6 @@ int init_traectory(traectory_fn f, coordpair_t *XY0){
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
}
if(ntries == 10) return FALSE;
XYcor.X.val = mdata.motXposition.val - mdata.encXposition.val;
XYcor.Y.val = mdata.motYposition.val - mdata.encYposition.val;
DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X.val), DEG2RAD(XYcor.Y.val));
return TRUE;
}
@@ -83,8 +76,8 @@ int telpos(coordval_pair_t *curpos){
}
if(ntries == 10) return FALSE;
coordval_pair_t pt;
pt.X.val = XYcor.X.val + mdata.encXposition.val;
pt.Y.val = XYcor.Y.val + mdata.encYposition.val;
pt.X.val = mdata.encXposition.val;
pt.Y.val = mdata.encYposition.val;
pt.X.t = mdata.encXposition.t;
pt.Y.t = mdata.encYposition.t;
if(curpos) *curpos = pt;
@@ -94,7 +87,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;
pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
if(nextpt) *nextpt = pt;
return TRUE;
@@ -103,7 +96,7 @@ int Linear(coordpair_t *nextpt, double t){
// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
int SinCos(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
if(nextpt) *nextpt = pt;
return TRUE;
@@ -116,8 +109,8 @@ typedef struct{
} tr_names;
static tr_names names[] = {
{Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"},
{SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
{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)"},
{NULL, NULL, NULL}
};