diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c index 123c5ae..5b818f6 100644 --- a/LibSidServo/examples/dump.c +++ b/LibSidServo/examples/dump.c @@ -184,8 +184,8 @@ void chk0(int ncycles){ if(!getPos(&M, NULL)) signals(2); if(M.X.val || M.Y.val){ WARNX("Mount position isn't @ zero; moving"); - double zero = 0.; - Mount.moveTo(&zero, &zero); + coordpair_t zero = {0., 0.}; + Mount.moveTo(&zero); waitmoving(ncycles); green("Now mount @ zero\n"); } diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index ee7f33f..f979590 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/LibSidServo/examples/dumpmoving.c @@ -92,11 +92,12 @@ int main(int argc, char **argv){ signal(SIGINT, signals); // ctrl+C - quit signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z - double tagx = DEG2RAD(45.) + M.X.val, tagy = DEG2RAD(45.) + M.Y.val; - if(MCC_E_OK != Mount.moveTo(&tagx, &tagy)) + coordpair_t tag = {.X = DEG2RAD(45.) + M.X.val, .Y = DEG2RAD(45.) + M.Y.val}; + if(MCC_E_OK != Mount.moveTo(&tag)) ERRX("Can't move to 45, 45"); dumpmoving(fcoords, 30., G.Ncycles); - Mount.moveTo(&M.X.val, &M.Y.val); + tag.X = M.X.val; tag.Y = M.Y.val; + Mount.moveTo(&tag); dumpmoving(fcoords, 30., G.Ncycles); signals(0); return 0; diff --git a/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c index bfb22ae..f16ee4b 100644 --- a/LibSidServo/examples/dumpmoving_scmd.c +++ b/LibSidServo/examples/dumpmoving_scmd.c @@ -168,7 +168,8 @@ int main(int argc, char **argv){ // and go back with 7deg/s move(0., 0., 7.); // be sure to move @ starting position - Mount.moveTo(&M.X.val, &M.Y.val); + coordpair_t tag = {.X = M.X.val, .Y = M.Y.val}; + Mount.moveTo(&tag); // wait moving ends pthread_join(dthr, NULL); signals(0); diff --git a/LibSidServo/examples/dumpswing.c b/LibSidServo/examples/dumpswing.c index 8425995..da65bef 100644 --- a/LibSidServo/examples/dumpswing.c +++ b/LibSidServo/examples/dumpswing.c @@ -146,24 +146,25 @@ int main(int argc, char **argv){ tagX = 0.; tagY = DEG2RAD(G.amplitude); } double t = Mount.currentT(), t0 = t; - double divide = 2., rtagX = -tagX, rtagY = -tagY; + coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY}; + double divide = 2.; for(int i = 0; i < G.Nswings; ++i){ - Mount.moveTo(&tagX, &tagY); + Mount.moveTo(&tag); DBG("CMD: %g", Mount.currentT()-t0); t += G.period / divide; divide = 1.; waithalf(t); DBG("Moved to +, t=%g", t-t0); DBG("CMD: %g", Mount.currentT()-t0); - Mount.moveTo(&rtagX, &rtagY); + Mount.moveTo(&rtag); t += G.period; waithalf(t); DBG("Moved to -, t=%g", t-t0); DBG("CMD: %g", Mount.currentT()-t0); } - double zero = 0.; + tag = (coordpair_t){.X = 0., .Y = 0.}; // be sure to move @ 0,0 - Mount.moveTo(&zero, &zero); + Mount.moveTo(&tag); // wait moving ends pthread_join(dthr, NULL); #undef SCMD diff --git a/LibSidServo/examples/goto.c b/LibSidServo/examples/goto.c index 24255aa..1038926 100644 --- a/LibSidServo/examples/goto.c +++ b/LibSidServo/examples/goto.c @@ -88,8 +88,8 @@ int main(int _U_ argc, char _U_ **argv){ return 1; } if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount"); - coordval_pair_t M; - if(!getPos(&M, NULL)) ERRX("Can't get current position"); + coordval_pair_t M, E; + if(!getPos(&M, &E)) ERRX("Can't get current position"); if(G.coordsoutput){ if(!G.wait) green("When logging I should wait until moving ends; added '-w'"); G.wait = 1; @@ -100,28 +100,27 @@ int main(int _U_ argc, char _U_ **argv){ logmnt(fcoords, NULL); if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); } - printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val)); + M.X.val = RAD2DEG(M.X.val); + M.Y.val = RAD2DEG(M.Y.val); + printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", M.X.val, M.Y.val, + RAD2DEG(E.X.val), RAD2DEG(E.Y.val)); if(isnan(G.X) && isnan(G.Y)) goto out; - double *xtag = NULL, *ytag = NULL, xr, yr; - double _7deg = RAD2DEG(7.); - if(!isnan(G.X)){ - xr = DEG2RAD(G.X); - if(G.relative) xr += M.X.val; - xtag = &xr; - // set max speed - Mount.setSpeed(&_7deg, NULL); + coordpair_t tag; + if(isnan(G.X)){ + if(G.relative) G.X = 0.; + else G.X = M.X.val; } - if(!isnan(G.Y)){ - yr = DEG2RAD(G.Y); - if(G.relative) yr += M.Y.val; - ytag = &yr; - Mount.setSpeed(NULL, &_7deg); + if(isnan(G.Y)){ + if(G.relative) G.Y = 0.; + else G.Y = M.Y.val; } - printf("Moving to "); - if(xtag) printf("X=%gdeg ", G.X); - if(ytag) printf("Y=%gdeg", G.Y); - printf("\n"); - Mount.moveTo(xtag, ytag); + if(G.relative){ + G.X += M.X.val; + G.Y += M.Y.val; + } + printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y); + tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y); + Mount.moveTo(&tag); if(G.wait){ sleep(1); waitmoving(G.Ncycles); diff --git a/LibSidServo/libsidservo.config b/LibSidServo/libsidservo.config index a8b7161..128fef3 100644 --- a/LibSidServo/libsidservo.config +++ b/LibSidServo/libsidservo.config @@ -1,6 +1,6 @@ // Add predefined macros for your project here. For example: // #define THE_ANSWER 42 #define EBUG -// #define _POSIX_C_SOURCE 111 +#define _POSIX_C_SOURCE 111 #define PACKAGE_VERSION "0.0.1" diff --git a/LibSidServo/main.c b/LibSidServo/main.c index f9d6518..e4ce2ba 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -26,6 +26,8 @@ conf_t Conf = {0}; +static mcc_errcodes_t shortcmd(short_command_t *cmd); + /** * @brief quit - close all opened and return to default state */ @@ -74,11 +76,29 @@ static mcc_errcodes_t init(conf_t *c){ data_t d = {.buf = buf, .len = 0, .maxlen = 1024}; // read input data as there may be some trash on start if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED; - if(ret != MCC_E_OK) quit(); + if(ret != MCC_E_OK) return ret; + mountdata_t md = {0}; + ret = MCC_E_FATAL; + double t0 = dtime(), t = 0.; + do{ + t = dtime(); + if(MCC_E_OK == getMD(&md)){ + if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){ + DBG("FIX motors position to encoders"); + int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val); + if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){ + DBG("All OK"); + ret = MCC_E_OK; + break; + } + } + } + DBG("NO DATA; dt = %g", t - t0); + }while(t - t0 < 2.); return ret; } -// check coordinates and speeds; return FALSE if failed +// check coordinates (rad) and speeds (rad/s); return FALSE if failed // TODO fix to real limits!!! static int chkX(double X){ if(X > 2.*M_PI || X < -2.*M_PI) return FALSE; @@ -89,11 +109,11 @@ static int chkY(double Y){ return TRUE; } static int chkXs(double s){ - if(s < 0. || s > X_SPEED_MAX) return FALSE; + if(s < 0. || s > MCC_MAX_X_SPEED) return FALSE; return TRUE; } static int chkYs(double s){ - if(s < 0. || s > Y_SPEED_MAX) return FALSE; + if(s < 0. || s > MCC_MAX_Y_SPEED) return FALSE; return TRUE; } @@ -110,41 +130,30 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){ * @param Y - new Y coordinate (radians: -pi..pi) or NULL * @return error code */ -static mcc_errcodes_t move2(const double *X, const double *Y){ - if(!X && !Y) return MCC_E_BADFORMAT; - if(X){ - if(!chkX(*X)) return MCC_E_BADFORMAT; - int32_t tag = X_RAD2MOT(*X); - DBG("X: %g, tag: %d", *X, tag); - if(!SSsetterI(CMD_MOTX, tag)) return MCC_E_FAILED; - } - if(Y){ - if(!chkY(*Y)) return MCC_E_BADFORMAT; - int32_t tag = Y_RAD2MOT(*Y); - DBG("Y: %g, tag: %d", *Y, tag); - if(!SSsetterI(CMD_MOTY, tag)) return MCC_E_FAILED; - } - return MCC_E_OK; +static mcc_errcodes_t move2(const coordpair_t *target){ + if(!target) return MCC_E_BADFORMAT; + if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; + short_command_t cmd = {0}; + DBG("x,y: %g, %g", target->X, target->Y); + cmd.Xmot = target->X; + cmd.Ymot = target->Y; + cmd.Xspeed = MCC_MAX_X_SPEED; + cmd.Yspeed = MCC_MAX_Y_SPEED; + return shortcmd(&cmd); } /** - * @brief setspeed - set maximal speed over axis + * @brief setspeed - set maximal speed over axis by text command * @param X (i) - max speed or NULL * @param Y (i) - -//- * @return errcode */ -static mcc_errcodes_t setspeed(const double *X, const double *Y){ - if(!X && !Y) return MCC_E_BADFORMAT; - if(X){ - if(!chkXs(*X)) return MCC_E_BADFORMAT; - int32_t spd = X_RS2MOTSPD(*X); - if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED; - } - if(Y){ - if(!chkYs(*Y)) return MCC_E_BADFORMAT; - int32_t spd = Y_RS2MOTSPD(*Y); - if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED; - } +static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){ + if(!tagspeed || !chkXs(tagspeed->X) || !chkYs(tagspeed->Y)) return MCC_E_BADFORMAT; + int32_t spd = X_RS2MOTSPD(tagspeed->X); + if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED; + spd = Y_RS2MOTSPD(tagspeed->Y); + if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED; return MCC_E_OK; } @@ -155,19 +164,15 @@ static mcc_errcodes_t setspeed(const double *X, const double *Y){ * @return */ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){ - if(!target && !speed) return MCC_E_BADFORMAT; - if(!target) return setspeed(&speed->X, &speed->Y); - if(!speed) return move2(&target->X, &target->Y); - if(!chkX(target->X) || !chkY(target->Y) || !chkXs(speed->X) || !chkYs(speed->Y)) - return MCC_E_BADFORMAT; - char buf[128]; - int32_t spd = X_RS2MOTSPD(speed->X), tag = X_RAD2MOT(target->X); - snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, CMD_MOTX, tag, CMD_MOTXYS, spd); - if(!SStextcmd(buf, NULL)) return MCC_E_FAILED; - spd = Y_RS2MOTSPD(speed->Y); tag = Y_RAD2MOT(target->Y); - snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, CMD_MOTY, tag, CMD_MOTXYS, spd); - if(!SStextcmd(buf, NULL)) return MCC_E_FAILED; - return MCC_E_OK; + if(!target || !speed) return MCC_E_BADFORMAT; + if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT; + if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT; + short_command_t cmd = {0}; + cmd.Xmot = target->X; + cmd.Ymot = target->Y; + cmd.Xspeed = speed->X; + cmd.Yspeed = speed->Y; + return shortcmd(&cmd); } /** @@ -192,7 +197,7 @@ static mcc_errcodes_t stop(){ static mcc_errcodes_t shortcmd(short_command_t *cmd){ if(!cmd) return MCC_E_BADFORMAT; SSscmd s = {0}; - DBG("xmot=%g, ymot=%g", cmd->Xmot, cmd->Ymot); + DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot); s.Xmot = X_RAD2MOT(cmd->Xmot); s.Ymot = Y_RAD2MOT(cmd->Ymot); s.Xspeed = X_RS2MOTSPD(cmd->Xspeed); diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index 5bebab9..6631f92 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -492,6 +492,8 @@ void closeSerial(){ if(mntfd > -1){ DBG("Kill mount thread"); pthread_cancel(mntthread); + DBG("join mount thread"); + pthread_join(mntthread, NULL); DBG("close fd"); close(mntfd); mntfd = -1; @@ -499,6 +501,8 @@ void closeSerial(){ if(encfd[0] > -1){ DBG("Kill encoder thread"); pthread_cancel(encthread); + DBG("join encoder thread"); + pthread_join(encthread, NULL); DBG("close fd"); close(encfd[0]); encfd[0] = -1; diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 44a5e94..21d32ce 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -27,6 +27,10 @@ extern "C" #include #include +// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s +#define MCC_MAX_X_SPEED (0.174533) +#define MCC_MAX_Y_SPEED (0.139626) + // max speed interval, seconds #define MCC_CONF_MAX_SPEEDINT (2.) // minimal speed interval in parts of EncoderReqInterval @@ -202,7 +206,7 @@ typedef struct{ // TODO: change (or add flags) switching slew-and-stop and slew-and-track // add mount state: stop/slew/guide mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags); - mcc_errcodes_t (*correctBy)(coordval_pair_t *delta); // delta = Target - Real + mcc_errcodes_t (*correctBy)(coordpair_t *delta); // delta = Target - Real mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index ae8ff2f..5dfc582 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -178,10 +178,6 @@ //#define Y_MOT_STEPSPERREV (4394960.) #define Y_MOT_STEPSPERREV (4394667.) -// maximal speeds in rad/s: 10deg/s by X and 8deg/s by Y -#define X_SPEED_MAX (0.17453) -#define Y_SPEED_MAX (0.13963) - // motor position to radians and back #define X_MOT2RAD(n) (2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV) #define Y_MOT2RAD(n) (2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV) @@ -206,11 +202,15 @@ #define X_ENC_STEPSPERREV (67108864.) #define Y_ENC_STEPSPERREV (67108864.) // encoder zero position -#define X_ENC_ZERO (46033555) +//#define X_ENC_ZERO (46033555) +#define X_ENC_ZERO (4603355) #define Y_ENC_ZERO (36674010) +// encoder reversed (no: +1) +#define X_ENC_SIGN (-1.) +#define Y_ENC_SIGN (-1.) // encoder position to radians and back -#define X_ENC2RAD(n) (2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV) -#define Y_ENC2RAD(n) (2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV) +#define X_ENC2RAD(n) (X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV) +#define Y_ENC2RAD(n) (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 Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))