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
Show Stats 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){ double pid_calculate(PIDController_t *pid, double error, double dt){
// calculate flowing integral // calculate flowing integral
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt; 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; pid->pidIarray[pid->curIidx++] = newi;
if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0; if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
pid->integral += newi - oldi; pid->integral += newi - oldi;
@ -71,68 +71,79 @@ typedef struct{
} PIDpair_t; } PIDpair_t;
typedef struct{ typedef struct{
axis_status_t *state; axis_status_t state;
coordval_t position; coordval_t position;
coordval_t speed; coordval_t speed;
} axisdata_t; } axisdata_t;
/** /**
* @brief process - Process PID for given axe * @brief process - Process PID for given axis
* @param tagpos - given coordinate of target position * @param tagpos - given coordinate of target position
* @param endpoint - endpoint for this coordinate * @param endpoint - endpoint for this coordinate
* @param pid - pid itself * @param pid - pid itself
* @return calculated new speed or -1 for max speed * @return calculated new speed or -1 for max speed
*/ */
static double getspeed(const coordval_t *tagpos, PIDpair_t *pidpair, axisdata_t *axis){ 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); double error = tagpos->val - axis->position.val, fe = fabs(error);
PIDController_t *pid = NULL; PIDController_t *pid = NULL;
switch(*axis->state){ switch(axis->state){
case AXIS_SLEWING: case AXIS_SLEWING:
if(fe < MCC_MAX_POINTING_ERR){ if(fe < MCC_MAX_POINTING_ERR){
*axis->state = AXIS_POINTING; axis->state = AXIS_POINTING;
DBG("--> Pointing\n"); DBG("--> Pointing");
pid = pidpair->PIDC; pid = pidpair->PIDC;
}else{ }else{
DBG("Slewing...\n"); DBG("Slewing...");
return -1.; // max speed for given axis return -1.; // max speed for given axis
} }
break; break;
case AXIS_POINTING: case AXIS_POINTING:
if(fe < MCC_MAX_GUIDING_ERR){ if(fe < MCC_MAX_GUIDING_ERR){
*axis->state = AXIS_GUIDING; axis->state = AXIS_GUIDING;
DBG("--> Guiding\n"); DBG("--> Guiding");
pid = pidpair->PIDV; pid = pidpair->PIDV;
}else if(fe > MCC_MAX_POINTING_ERR){ }else if(fe > MCC_MAX_POINTING_ERR){
DBG("--> Slewing\n"); DBG("--> Slewing");
*axis->state = AXIS_SLEWING; axis->state = AXIS_SLEWING;
return -1.; return -1.;
} else pid = pidpair->PIDC; } else pid = pidpair->PIDC;
break; break;
case AXIS_GUIDING: case AXIS_GUIDING:
pid = pidpair->PIDV; pid = pidpair->PIDV;
if(fe > MCC_MAX_GUIDING_ERR){ if(fe > MCC_MAX_GUIDING_ERR){
DBG("--> Pointing\n"); DBG("--> Pointing");
*axis->state = AXIS_POINTING; axis->state = AXIS_POINTING;
pid = pidpair->PIDC; pid = pidpair->PIDC;
}else if(fe < MCC_MAX_ATTARGET_ERR){ }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 // 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; 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: case AXIS_ERROR:
DBG("Can't move from erroneous state");
return 0.; return 0.;
} }
if(!pid){ if(!pid){
DBG("WTF? Where is a PID?"); DBG("WTF? Where is a PID?");
return axis->speed.val; 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; double dt = tagpos->t - pid->prevT;
if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME; if(dt > MCC_PID_MAX_DT) dt = MCC_PID_CYCLE_TIME;
pid->prevT = tagpos->t; pid->prevT = tagpos->t;
//DBG("CALC PID (er=%g, dt=%g)", error, dt);
double tagspeed = pid_calculate(pid, 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 return tagspeed; // coordinate-based
} }
@ -159,16 +170,24 @@ mcc_errcodes_t correct2(const coordval_pair_t *target, const coordpair_t *endpoi
mountdata_t m; mountdata_t m;
coordpair_t tagspeed; coordpair_t tagspeed;
if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED; if(MCC_E_OK != Mount.getMountData(&m)) return MCC_E_FAILED;
axisdata_t axe; axisdata_t axis;
axe.state = &m.Xstate; DBG("state: %d/%d", m.Xstate, m.Ystate);
axe.position = m.encXposition; axis.state = m.Xstate;
axe.speed = m.encXspeed; axis.position = m.encXposition;
tagspeed.X = getspeed(&target->X, &pidX, &axe); 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 > MCC_MAX_X_SPEED) tagspeed.X = MCC_MAX_X_SPEED;
axe.state = &m.Ystate; axis_status_t xstate = axis.state;
axe.position = m.encYposition; axis.state = m.Ystate;
axe.speed = m.encYspeed; axis.position = m.encYposition;
tagspeed.Y = getspeed(&target->Y, &pidY, &axe); 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 > 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); return Mount.moveWspeed(endpoint, &tagspeed);
} }

16
LibSidServo/examples/SSIIconf.c
View File

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

18
LibSidServo/examples/conf.c
View File

@ -31,14 +31,18 @@ static conf_t Config = {
.EncoderReqInterval = 0.05, .EncoderReqInterval = 0.05,
.SepEncoder = 2, .SepEncoder = 2,
.EncoderSpeedInterval = 0.1, .EncoderSpeedInterval = 0.1,
.XPIDC.P = 0.5, .XPIDC.P = 0.8,
.XPIDC.D = 0.05, .XPIDC.I = 0.1,
.XPIDV.P = 0.5, .XPIDC.D = 0.3,
.XPIDV.D = 0.05, .XPIDV.P = 1.,
.YPIDC.P = 0.5, .XPIDV.I = 0.01,
.YPIDC.D = 0.05, .XPIDV.D = 0.2,
.YPIDC.P = 0.8,
.YPIDC.I = 0.1,
.YPIDC.D = 0.3,
.YPIDV.P = 0.5, .YPIDV.P = 0.5,
.YPIDV.D = 0.05, .YPIDV.I = 0.2,
.YPIDV.D = 0.5,
}; };
static sl_option_t opts[] = { 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 * @param N - amount of stopped cycles
*/ */
void waitmoving(int N){ 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 X0; // starting point of traectory (-30..30 degr)
double Y0; // -//- double Y0; // -//-
char *coordsoutput; // dump file char *coordsoutput; // dump file
char *errlog; // log with position errors
char *tfn; // traectory function name char *tfn; // traectory function name
char *conffile; char *conffile;
} parameters; } parameters;
static FILE *fcoords = NULL; static FILE *fcoords = NULL, *errlog = NULL;
static pthread_t dthr; static pthread_t dthr;
static parameters G = { static parameters G = {
.Ncycles = 40, .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"}, {"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)"}, {"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)"}, {"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)"}, {"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 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)"}, {"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)"}, {"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)"}, {"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"}, {"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 end_option
}; };
@ -76,6 +78,8 @@ void signals(int sig){
signal(sig, SIG_IGN); signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig); DBG("Get signal %d, quit.\n", sig);
} }
Mount.stop();
sleep(1);
Mount.quit(); Mount.quit();
if(fcoords) fclose(fcoords); if(fcoords) fclose(fcoords);
exit(sig); exit(sig);
@ -90,22 +94,39 @@ static void *dumping(void _U_ *u){
static void runtraectory(traectory_fn tfn){ static void runtraectory(traectory_fn tfn){
if(!tfn) return; if(!tfn) return;
coordval_pair_t telXY; coordval_pair_t telXY;
coordpair_t traectXY; coordval_pair_t target;
double t0 = Mount.currentT(); coordpair_t traectXY, endpoint;
double tlast = 0.; endpoint.X = G.Xmax, endpoint.Y = G.Ymax;
double t0 = Mount.currentT(), tlast = 0.;
double tlastX = 0., tlastY = 0.;
while(1){ while(1){
if(!telpos(&telXY)){ if(!telpos(&telXY)){
WARNX("No next telescope position"); WARNX("No next telescope position");
return; return;
} }
if(telXY.X.t == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind if(telXY.X.t == tlastX && telXY.Y.t == tlastY) continue; // last measure - don't mind
tlast = (telXY.X.t + telXY.Y.t) / 2.; 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(); 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; 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){ 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); G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.) if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
ERRX("X0 and Y0 should be -30..30 degrees"); 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(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w"))) if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput); ERRX("Can't open %s", G.coordsoutput);

12
LibSidServo/examples/simpleconv.h
View File

@ -20,10 +20,10 @@
#include <math.h> #include <math.h>
#define DEG2RAD(d) (d/180.*M_PI) #define DEG2RAD(d) ((d)/180.*M_PI)
#define ASEC2RAD(d) (d/180.*M_PI/3600.) #define ASEC2RAD(d) ((d)/180.*M_PI/3600.)
#define AMIN2RAD(d) (d/180.*M_PI/60.) #define AMIN2RAD(d) ((d)/180.*M_PI/60.)
#define RAD2DEG(r) (r/M_PI*180.) #define RAD2DEG(r) ((r)/M_PI*180.)
#define RAD2ASEC(r) (r/M_PI*180.*3600.) #define RAD2ASEC(r) ((r)/M_PI*180.*3600.)
#define RAD2AMIN(r) (r/M_PI*180.*60.) #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 // starting point of traectory
static coordpair_t XYstart = {0}; static coordpair_t XYstart = {0};
static double tstart = 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 * @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(MCC_E_OK == Mount.getMountData(&mdata)) break;
} }
if(ntries == 10) return FALSE; 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; return TRUE;
} }
@ -83,8 +76,8 @@ int telpos(coordval_pair_t *curpos){
} }
if(ntries == 10) return FALSE; if(ntries == 10) return FALSE;
coordval_pair_t pt; coordval_pair_t pt;
pt.X.val = XYcor.X.val + mdata.encXposition.val; pt.X.val = mdata.encXposition.val;
pt.Y.val = XYcor.Y.val + mdata.encYposition.val; pt.Y.val = mdata.encYposition.val;
pt.X.t = mdata.encXposition.t; pt.X.t = mdata.encXposition.t;
pt.Y.t = mdata.encYposition.t; pt.Y.t = mdata.encYposition.t;
if(curpos) *curpos = pt; if(curpos) *curpos = pt;
@ -94,7 +87,7 @@ int telpos(coordval_pair_t *curpos){
// X=X0+1'/s, Y=Y0+15''/s // X=X0+1'/s, Y=Y0+15''/s
int Linear(coordpair_t *nextpt, double t){ int Linear(coordpair_t *nextpt, double t){
coordpair_t pt; 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); pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
if(nextpt) *nextpt = pt; if(nextpt) *nextpt = pt;
return TRUE; 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) // X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
int SinCos(coordpair_t *nextpt, double t){ int SinCos(coordpair_t *nextpt, double t){
coordpair_t pt; 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); pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
if(nextpt) *nextpt = pt; if(nextpt) *nextpt = pt;
return TRUE; return TRUE;
@ -116,8 +109,8 @@ typedef struct{
} tr_names; } tr_names;
static tr_names names[] = { static tr_names names[] = {
{Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"}, {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)"}, {SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
{NULL, NULL, NULL} {NULL, NULL, NULL}
}; };

27
LibSidServo/main.c
View File

@ -86,7 +86,6 @@ void getModData(mountdata_t *mountdata){
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow); if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp); movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow); if(Yst == ST_MOVE) Yst = Ymodel->proc_move(Ymodel, &Yp, tnow);
bzero(mountdata, sizeof(mountdata_t));
mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow; mountdata->motXposition.t = mountdata->encXposition.t = mountdata->motYposition.t = mountdata->encYposition.t = tnow;
mountdata->motXposition.val = mountdata->encXposition.val = Xp.coord; mountdata->motXposition.val = mountdata->encXposition.val = Xp.coord;
mountdata->motYposition.val = mountdata->encYposition.val = Yp.coord; mountdata->motYposition.val = mountdata->encYposition.val = Yp.coord;
@ -210,6 +209,22 @@ static int chkYs(double s){
return TRUE; return TRUE;
} }
// set SLEWING state if axis was stopped later
static void setslewingstate(){
//FNAME();
mountdata_t d;
if(MCC_E_OK == getMD(&d)){
axis_status_t newx = d.Xstate, newy = d.Ystate;
//DBG("old state: %d/%d", d.Xstate, d.Ystate);
if(d.Xstate == AXIS_STOPPED) newx = AXIS_SLEWING;
if(d.Ystate == AXIS_STOPPED) newy = AXIS_SLEWING;
if(newx != d.Xstate || newy != d.Ystate){
DBG("Started moving -> slew");
setStat(newx, newy);
}
}else DBG("CAN't GET MOUNT DATA!");
}
/* /*
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
(void)target; (void)target;
@ -241,7 +256,7 @@ static mcc_errcodes_t move2(const coordpair_t *target){
cmd.Yspeed = MCC_MAX_Y_SPEED; cmd.Yspeed = MCC_MAX_Y_SPEED;
mcc_errcodes_t r = shortcmd(&cmd); mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r; if(r != MCC_E_OK) return r;
setStat(AXIS_SLEWING, AXIS_SLEWING); setslewingstate();
return MCC_E_OK; return MCC_E_OK;
} }
@ -279,7 +294,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
cmd.Yspeed = speed->Y; cmd.Yspeed = speed->Y;
mcc_errcodes_t r = shortcmd(&cmd); mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r; if(r != MCC_E_OK) return r;
setStat(AXIS_SLEWING, AXIS_SLEWING); setslewingstate();
return MCC_E_OK; return MCC_E_OK;
} }
@ -288,6 +303,7 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
* @return errcode * @return errcode
*/ */
static mcc_errcodes_t emstop(){ static mcc_errcodes_t emstop(){
FNAME();
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = nanotime();
Xmodel->emergency_stop(Xmodel, curt); Xmodel->emergency_stop(Xmodel, curt);
@ -299,6 +315,7 @@ static mcc_errcodes_t emstop(){
} }
// normal stop // normal stop
static mcc_errcodes_t stop(){ static mcc_errcodes_t stop(){
FNAME();
if(Conf.RunModel){ if(Conf.RunModel){
double curt = nanotime(); double curt = nanotime();
Xmodel->stop(Xmodel, curt); Xmodel->stop(Xmodel, curt);
@ -323,6 +340,7 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED; if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = cmd->Ymot; param.speed = cmd->Yspeed; param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED; if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
setslewingstate();
return MCC_E_OK; return MCC_E_OK;
} }
SSscmd s = {0}; SSscmd s = {0};
@ -336,6 +354,7 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
s.YBits = cmd->YBits; s.YBits = cmd->YBits;
DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed); DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed);
if(!cmdS(&s)) return MCC_E_FAILED; if(!cmdS(&s)) return MCC_E_FAILED;
setslewingstate();
return MCC_E_OK; return MCC_E_OK;
} }
@ -353,6 +372,7 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED; if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = cmd->Ymot; param.speed = cmd->Yspeed; param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED; if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
setslewingstate();
return MCC_E_OK; return MCC_E_OK;
} }
SSlcmd l = {0}; SSlcmd l = {0};
@ -365,6 +385,7 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
l.Xatime = S2ADDER(cmd->Xatime); l.Xatime = S2ADDER(cmd->Xatime);
l.Yatime = S2ADDER(cmd->Yatime); l.Yatime = S2ADDER(cmd->Yatime);
if(!cmdL(&l)) return MCC_E_FAILED; if(!cmdL(&l)) return MCC_E_FAILED;
setslewingstate();
return MCC_E_OK; return MCC_E_OK;
} }

2
LibSidServo/main.h
View File

@ -69,7 +69,7 @@ double LS_calc_slope(less_square_t *l, double x, double t);
#define COLOR_OLD "\033[0;0;0m" #define COLOR_OLD "\033[0;0;0m"
#define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \ #define FNAME() do{ fprintf(stderr, COLOR_GREEN "\n%s " COLOR_OLD, __func__); \
fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0) fprintf(stderr, "(%s, line %d)\n", __FILE__, __LINE__);} while(0)
#define DBG(...) do{ fprintf(stderr, COLOR_RED "\n%s " COLOR_OLD, __func__); \ #define DBG(...) do{ fprintf(stderr, COLOR_RED "%s " COLOR_OLD, __func__); \
fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \ fprintf(stderr, "(%s, line %d): ", __FILE__, __LINE__); \
fprintf(stderr, __VA_ARGS__); \ fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n");} while(0) fprintf(stderr, "\n");} while(0)

3
LibSidServo/movingmodel.c
View File

@ -37,7 +37,10 @@ static void chkminmax(double *min, double *max){
movemodel_t *model_init(limits_t *l){ movemodel_t *model_init(limits_t *l){
if(!l) return FALSE; if(!l) return FALSE;
movemodel_t *m = calloc(1, sizeof(movemodel_t)); movemodel_t *m = calloc(1, sizeof(movemodel_t));
// we can't use memcpy or assign as Times/Params would be common for all
*m = trapez; *m = trapez;
m->Times = calloc(STAGE_AMOUNT, sizeof(double));
m->Params = calloc(STAGE_AMOUNT, sizeof(moveparam_t));
moveparam_t *max = &l->max, *min = &l->min; moveparam_t *max = &l->max, *min = &l->min;
if(min->speed < 0.) min->speed = -min->speed; if(min->speed < 0.) min->speed = -min->speed;
if(max->speed < 0.) max->speed = -max->speed; if(max->speed < 0.) max->speed = -max->speed;

4
LibSidServo/movingmodel.h
View File

@ -60,8 +60,8 @@ typedef struct movemodel{
moveparam_t Max; moveparam_t Max;
movingstage_t movingstage; movingstage_t movingstage;
movestate_t state; movestate_t state;
double Times[STAGE_AMOUNT]; double *Times;
moveparam_t Params[STAGE_AMOUNT]; moveparam_t *Params;
moveparam_t curparams; // init values of limits, jerk moveparam_t curparams; // init values of limits, jerk
int (*calculate)(struct movemodel *m, moveparam_t *target, double t); // calculate stages of traectory beginning from t int (*calculate)(struct movemodel *m, moveparam_t *target, double t); // calculate stages of traectory beginning from t
movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t); // calculate next model point for time t movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t); // calculate next model point for time t

41
LibSidServo/ramp.c
View File

@ -23,7 +23,12 @@
#include "main.h" #include "main.h"
#include "ramp.h" #include "ramp.h"
/*
#ifdef EBUG
#undef DBG
#define DBG(...)
#endif
*/
static double coord_tolerance = COORD_TOLERANCE_DEFAULT; static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
static void emstop(movemodel_t *m, double _U_ t){ static void emstop(movemodel_t *m, double _U_ t){
@ -66,9 +71,9 @@ ret:
* @return FALSE if can't move with given parameters * @return FALSE if can't move with given parameters
*/ */
static int calc(movemodel_t *m, moveparam_t *x, double t){ static int calc(movemodel_t *m, moveparam_t *x, double t){
DBG("coord/speed: %g/%g", x->coord, x->speed);
if(!x) return FALSE; if(!x) return FALSE;
pthread_mutex_lock(&m->mutex); pthread_mutex_lock(&m->mutex);
DBG("target coord/speed: %g/%g; current coord: %g", x->coord, x->speed, m->curparams.coord);
int ret = FALSE; int ret = FALSE;
if(x->coord < m->Min.coord || x->coord > m->Max.coord){ if(x->coord < m->Min.coord || x->coord > m->Max.coord){
DBG("Wrong coordinage [%g, %g]", m->Min.coord, m->Max.coord); DBG("Wrong coordinage [%g, %g]", m->Min.coord, m->Max.coord);
@ -101,6 +106,33 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
ret = TRUE; ret = TRUE;
goto ret; goto ret;
} }
#if 0
model_move2 (/home/eddy/C-files/LibSidServo/movingmodel.c, line 63): MOVE to -0.785398 at speed 0.00408621
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 76): target coord/speed: -0.785398/0.00408621; current coord: 0.181616
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 91): Dx=0.967014, sign=-1, dt23=0.0185812, dx23=3.79634e-05
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 101): dt0s=0.0377057, dx0s=0.000156326, curspeed=0.0082919
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 110): SIGN of speed should be changed!
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 0: t=3.9028, coord=0.181616, speed=0.0082919, accel=-0.219911
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 1: t=3.9405, coord=0.181772, speed=0, accel=-0.219911
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 2: t=3.95908, coord=0.181734, speed=-0.00408621, accel=0.219911
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 3: t=3.97766, coord=-0.785398, speed=0, accel=0
model_move2 (/home/eddy/C-files/LibSidServo/movingmodel.c, line 63): MOVE to -0.785398 at speed 0.0361538
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 76): target coord/speed: -0.785398/0.0361538; current coord: 0.177343
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 91): Dx=0.962741, sign=-1, dt23=0.218049, dx23=0.00394164
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 101): dt0s=0.185651, dx0s=0.00285737, curspeed=0.0307821
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 110): SIGN of speed should be changed!
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 0: t=3.9028, coord=0.177343, speed=0.0307821, accel=-0.165806
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 1: t=4.08845, coord=0.1802, speed=0, accel=-0.165806
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 2: t=4.3065, coord=0.176259, speed=-0.0361538, accel=0.165806
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 3: t=4.52455, coord=-0.785398, speed=0, accel=0
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 233): REACHED STOPping stage @ t=3.97837
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[3]=3.97766,
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[2]=3.95908,
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[1]=3.9405,
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[0]=3.9028,
#endif
if(m->curparams.speed * sign < 0. || m->state == ST_STOP || (dx0s > Dx + coord_tolerance)){ // we should change speed sign if(m->curparams.speed * sign < 0. || m->state == ST_STOP || (dx0s > Dx + coord_tolerance)){ // we should change speed sign
DBG("SIGN of speed should be changed!"); DBG("SIGN of speed should be changed!");
double sign_current = (m->curparams.speed >= 0.) ? 1. : -1.; double sign_current = (m->curparams.speed >= 0.) ? 1. : -1.;
@ -225,6 +257,11 @@ static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
if(m->movingstage == STAGE_STOPPED){ if(m->movingstage == STAGE_STOPPED){
m->curparams.coord = m->Params[STAGE_STOPPED].coord; m->curparams.coord = m->Params[STAGE_STOPPED].coord;
pthread_mutex_unlock(&m->mutex); pthread_mutex_unlock(&m->mutex);
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); emstop(m, t);
goto ret; goto ret;
} }

11
LibSidServo/serial.c
View File

@ -243,7 +243,10 @@ static int getmntbyte(){
if(FD_ISSET(mntfd, &rfds)){ if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1); ssize_t l = read(mntfd, &byte, 1);
//DBG("MNT read=%zd byte=0x%X", l, byte); //DBG("MNT read=%zd byte=0x%X", l, byte);
if(l != 1) return -2; // disconnected ?? if(l != 1){
DBG("Mount disconnected?");
return -2; // disconnected ??
}
break; break;
} else return -1; } else return -1;
}while(1); }while(1);
@ -586,6 +589,7 @@ mcc_errcodes_t getMD(mountdata_t *d){
} }
void setStat(axis_status_t Xstate, axis_status_t Ystate){ void setStat(axis_status_t Xstate, axis_status_t Ystate){
DBG("set x/y state to %d/%d", Xstate, Ystate);
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
mountdata.Xstate = Xstate; mountdata.Xstate = Xstate;
mountdata.Ystate = Ystate; mountdata.Ystate = Ystate;
@ -594,7 +598,10 @@ void setStat(axis_status_t Xstate, axis_status_t Ystate){
// write-read without locking mutex (to be used inside other functions) // write-read without locking mutex (to be used inside other functions)
static int wr(const data_t *out, data_t *in, int needeol){ static int wr(const data_t *out, data_t *in, int needeol){
if((!out && !in) || mntfd < 0) return FALSE; if((!out && !in) || mntfd < 0){
DBG("Wrong arguments or no mount fd");
return FALSE;
}
clrmntbuf(); clrmntbuf();
//double t0 = nanotime(); //double t0 = nanotime();
if(out){ if(out){

21
LibSidServo/sidservo.h
View File

@ -34,7 +34,7 @@ extern "C"
// acceptable position error - 0.1'' // acceptable position error - 0.1''
#define MCC_POSITION_ERROR (5e-7) #define MCC_POSITION_ERROR (5e-7)
// acceptable disagreement between motor and axe encoders - 2'' // acceptable disagreement between motor and axis encoders - 2''
#define MCC_ENCODERS_ERROR (1e-7) #define MCC_ENCODERS_ERROR (1e-7)
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s // max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
@ -56,10 +56,11 @@ extern "C"
// normal PID refresh interval // normal PID refresh interval
#define MCC_PID_REFRESH_DT (0.1) #define MCC_PID_REFRESH_DT (0.1)
// boundary conditions for axis state: "slewing/pointing/guiding" // boundary conditions for axis state: "slewing/pointing/guiding"
// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 12 degrees // if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 5 degrees
#define MCC_MAX_POINTING_ERR (0.20943951) //#define MCC_MAX_POINTING_ERR (0.20943951)
// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "slewing": 15'' #define MCC_MAX_POINTING_ERR (0.08726646)
#define MCC_MAX_GUIDING_ERR (7.272205e-5) // 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'' // if error less than this value we suppose that target is captured and guiding is good: 0.1''
#define MCC_MAX_ATTARGET_ERR (4.8481368e-7) #define MCC_MAX_ATTARGET_ERR (4.8481368e-7)
@ -89,7 +90,7 @@ typedef struct{
double EncoderReqInterval; // interval between subsequent encoder requests (seconds) double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
double EncoderSpeedInterval; // interval between speed calculations double EncoderSpeedInterval; // interval between speed calculations
int RunModel; // == 1 if you want to use model instead of real mount int RunModel; // == 1 if you want to use model instead of real mount
PIDpar_t XPIDC; // gain parameters of PID for both axes (C - coordinate driven, V - velocity driven) PIDpar_t XPIDC; // gain parameters of PID for both axiss (C - coordinate driven, V - velocity driven)
PIDpar_t XPIDV; PIDpar_t XPIDV;
PIDpar_t YPIDC; PIDpar_t YPIDC;
PIDpar_t YPIDV; PIDpar_t YPIDV;
@ -193,7 +194,7 @@ typedef struct{
double Yatime; // 28 double Yatime; // 28
} long_command_t; // long command } long_command_t; // long command
// hardware axe configuration // hardware axis configuration
typedef struct{ typedef struct{
double accel; // Default Acceleration, rad/s^2 double accel; // Default Acceleration, rad/s^2
double backlash; // Backlash (???) double backlash; // Backlash (???)
@ -204,13 +205,13 @@ typedef struct{
double outplimit; // Output Limit, percent (0..100) double outplimit; // Output Limit, percent (0..100)
double currlimit; // Current Limit (A) double currlimit; // Current Limit (A)
double intlimit; // Integral Limit (???) double intlimit; // Integral Limit (???)
} __attribute__((packed)) axe_config_t; } __attribute__((packed)) axis_config_t;
// hardware configuration // hardware configuration
typedef struct{ typedef struct{
axe_config_t Xconf; axis_config_t Xconf;
xbits_t xbits; xbits_t xbits;
axe_config_t Yconf; axis_config_t Yconf;
ybits_t ybits; ybits_t ybits;
uint8_t address; uint8_t address;
double eqrate; // Equatorial Rate (???) double eqrate; // Equatorial Rate (???)

19
LibSidServo/ssii.c
View File

@ -36,19 +36,18 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
return checksum; return checksum;
} }
// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
static void axestat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){ static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
if(*prev == INT32_MAX){ if(*prev == INT32_MAX){
*stat = AXIS_STOPPED; *stat = AXIS_STOPPED;
DBG("START"); DBG("START");
}else if(*stat != AXIS_STOPPED){ }else if(*stat != AXIS_STOPPED){
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){ if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
*stat = AXIS_STOPPED; *stat = AXIS_STOPPED;
DBG("AXE stopped"); DBG("AXIS stopped");
} }
}else if(*prev != cur){ }else if(*prev != cur){
DBG("AXE moving"); DBG("AXIS moving");
//*stat = AXIS_SLEWING;
*nstopped = 0; *nstopped = 0;
} }
*prev = cur; *prev = cur;
@ -57,8 +56,8 @@ static void axestat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *st
static void ChkStopped(const SSstat *s, mountdata_t *m){ static void ChkStopped(const SSstat *s, mountdata_t *m){
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate); chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate); chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
} }
/** /**
@ -191,14 +190,14 @@ mcc_errcodes_t updateMotorPos(){
DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t); DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
mcc_errcodes_t OK = MCC_E_OK; mcc_errcodes_t OK = MCC_E_OK;
if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync X: motors=%g, axes=%g", md.motXposition.val, md.encXposition.val); DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val);
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){ if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
DBG("Xpos sync failed!"); DBG("Xpos sync failed!");
OK = MCC_E_FAILED; OK = MCC_E_FAILED;
}else DBG("Xpos sync OK, Dt=%g", nanotime() - t0); }else DBG("Xpos sync OK, Dt=%g", nanotime() - t0);
} }
if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){ if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync Y: motors=%g, axes=%g", md.motYposition.val, md.encYposition.val); DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val);
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){ if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
DBG("Ypos sync failed!"); DBG("Ypos sync failed!");
OK = MCC_E_FAILED; OK = MCC_E_FAILED;

4
LibSidServo/ssii.h
View File

@ -194,8 +194,8 @@
#define X_ENC_SIGN (-1.) #define X_ENC_SIGN (-1.)
#define Y_ENC_SIGN (-1.) #define Y_ENC_SIGN (-1.)
// encoder position to radians and back // encoder position to radians and back
#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV) #define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV) #define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)((n)-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV)) #define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV)) #define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))