diff --git a/Auxiliary_utils/LibSidServo/TODO b/Auxiliary_utils/LibSidServo/TODO index c636b16..b6b7a6a 100644 --- a/Auxiliary_utils/LibSidServo/TODO +++ b/Auxiliary_utils/LibSidServo/TODO @@ -9,3 +9,7 @@ closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 491): close fd Aborted +2. Init: set "motors" positions due to "encoders" position + +3. From time to time: repeat 2 + diff --git a/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt b/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt index 944628d..5afbbd6 100644 --- a/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt +++ b/Auxiliary_utils/LibSidServo/examples/CMakeLists.txt @@ -11,3 +11,4 @@ add_executable(dump_s dumpmoving_scmd.c dump.c conf.c) add_executable(dumpswing dumpswing.c dump.c conf.c) add_executable(traectory_s scmd_traectory.c dump.c traectories.c conf.c) add_executable(SSIIconf SSIIconf.c conf.c) +add_executable(slewNtrack dumpmoving_dragNtrack.c dump.c conf.c) diff --git a/Auxiliary_utils/LibSidServo/examples/dump.c b/Auxiliary_utils/LibSidServo/examples/dump.c index a7d9ef6..123c5ae 100644 --- a/Auxiliary_utils/LibSidServo/examples/dump.c +++ b/Auxiliary_utils/LibSidServo/examples/dump.c @@ -115,7 +115,7 @@ void dumpmoving(FILE *fcoords, double t, int N){ enct = tmsr; if(fcoords) logmnt(fcoords, &mdata); if(mdata.millis == mdmillis) continue; - DBG("ctr=%d", ctr); + //DBG("ctr=%d", ctr); mdmillis = mdata.millis; if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){ xlast = mdata.motXposition.val; diff --git a/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c b/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c index 006aabd..326ec5e 100644 --- a/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c +++ b/Auxiliary_utils/LibSidServo/examples/dumpmoving_dragNtrack.c @@ -17,6 +17,7 @@ */ // move telescope to target using short command and force it to track mode +// also do some corrections while moving #include #include @@ -31,42 +32,43 @@ #include "sidservo.h" #include "simpleconv.h" +// Original XXI=6827 +// XXD=136546 +// XXB=4915666 + typedef struct{ int help; int Ncycles; - int relative; double reqint; char *coordsoutput; char *conffile; - char *axis; } parameters; static parameters G = { .Ncycles = 40, .reqint = -1., - .axis = "X", }; static FILE *fcoords = NULL; -static coords_t M; - static sl_option_t cmdlnopts[] = { {"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"}, {"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"}, {"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)"}, - {"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X, Y or B for both)"}, {"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"}, - {"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"}, end_option }; +static mcc_errcodes_t return2zero(); + void signals(int sig){ if(sig){ signal(sig, SIG_IGN); DBG("Get signal %d, quit.\n", sig); } + return2zero(); + sleep(5); Mount.quit(); exit(sig); } @@ -78,63 +80,90 @@ static void *dumping(void _U_ *u){ } // return TRUE if motor position is reached +- 0.01 degrees -#define XYcount (DEG2RAD(0.01)) -static int Wait(double tag){ +#define XYcount (DEG2RAD(0.3)) +// tag in degrees! +static int Wait(double tag, int isX){ mountdata_t mdata; - red("Wait for %g degrees\n", RAD2DEG(tag)); + red("Wait for %g degrees\n", tag); + tag = DEG2RAD(tag); int errcnt = 0; - double sign = 0.; uint32_t millis = 0; double curpos = 0.; + double t0 = sl_dtime(); do{ if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt; else{ errcnt = 0; if(mdata.millis == millis) continue; millis = mdata.millis; - if(*G.axis == 'X') curpos = mdata.motposition.X; - else curpos = mdata.motposition.Y; - if(sign == 0.) sign = (curpos > tag) ? 1. : -1.; - //printf("%s=%g deg, need %g deg; delta=%g arcmin\n", G.axis, RAD2DEG(curpos), - // RAD2DEG(tag), RAD2DEG(sign*(curpos - tag))*60.); + if(isX) curpos = mdata.motXposition.val; + else curpos = mdata.motYposition.val; } - }while(sign*(curpos - tag) > XYcount && errcnt < 10); + double t = sl_dtime(); + if(t - t0 > 1.){ + t0 = t; + printf("\t\tCurrent MOT X/Y: %g / %g deg\n", RAD2DEG(mdata.motXposition.val), + RAD2DEG(mdata.motYposition.val)); + } + }while(fabs(curpos - tag) > XYcount && errcnt < 10); if(errcnt >= 10){ WARNX("Too much errors"); return FALSE; } - green("%s reached position %g degrees\n", G.axis, RAD2DEG(tag)); + green("%s reached position %g degrees\n", (isX) ? "X" : "Y", RAD2DEG(tag)); fflush(stdout); return TRUE; } -// move X/Y to 40 degr with given speed until given coord -static void move(double target, double limit, double speed){ - green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed); +// previous GOTO coords/speeds for `mkcorr` +static coordpair_t lastTag = {0}, lastSpeed = {0}; + +// slew to given position and start tracking +// pos/speed in deg and deg/s +static mcc_errcodes_t gotos(coordpair_t *target, coordpair_t *speed){ short_command_t cmd = {0}; - if(*G.axis == 'X' || *G.axis == 'B'){ - cmd.Xmot = DEG2RAD(target) + M.X; - cmd.Xspeed = DEG2RAD(speed); - limit = DEG2RAD(limit) + M.X; - } - if(*G.axis == 'Y' || *G.axis == 'B'){ - cmd.Ymot = DEG2RAD(target) + M.Y; - cmd.Yspeed = DEG2RAD(speed); - if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y; - } - if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); - if(!Wait(limit)) signals(9); + DBG("Try to move to (%g, %g) with speed (%g, %g)", + target->X, target->Y, speed->X, speed->Y); + target->X = DEG2RAD(target->X); + target->Y = DEG2RAD(target->Y); + speed->X = DEG2RAD(speed->X); + speed->Y = DEG2RAD(speed->Y); + cmd.Xmot = target->X; cmd.Ymot = target->Y; + cmd.Xspeed = speed->X; cmd.Yspeed = speed->Y; + lastTag = *target; + lastSpeed = *speed; + /*cmd.xychange = 1; + cmd.XBits = 108; + cmd.YBits = 28;*/ + return Mount.shortCmd(&cmd); } +static mcc_errcodes_t return2zero(){ + short_command_t cmd = {0}; + DBG("Try to move to zero"); + cmd.Xmot = 0.; cmd.Ymot = 0.; + cmd.Xspeed = DEG2RAD(10.); cmd.Yspeed = DEG2RAD(10.); + /*cmd.xychange = 1; + cmd.XBits = 100; + cmd.YBits = 20;*/ + return Mount.shortCmd(&cmd); +} + +static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){ + long_command_t cmd = {0}; + cmd.Xspeed = lastSpeed.X; + cmd.Yspeed = lastSpeed.Y; + cmd.Xmot = lastTag.X; + cmd.Ymot = lastTag.Y; + cmd.Xadder = adder->X; cmd.Yadder = adder->Y; + cmd.Xatime = time->X; cmd.Yatime = time->Y; + return Mount.longCmd(&cmd); +} int main(int argc, char **argv){ sl_init(); sl_parseargs(&argc, &argv, cmdlnopts); if(G.help) sl_showhelp(-1, cmdlnopts); - if(strcmp(G.axis, "X") && strcmp(G.axis, "Y") && strcmp(G.axis, "B")){ - WARNX("\"Axis\" should be X, Y or B"); - return 1; - } if(G.coordsoutput){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); @@ -149,7 +178,7 @@ int main(int argc, char **argv){ WARNX("Can't init devices"); return 1; } - if(!getPos(&M, NULL)) ERRX("Can't get current position"); + //if(!getPos(&M, NULL)) ERRX("Can't get current position"); signal(SIGTERM, signals); // kill (-15) - quit signal(SIGHUP, SIG_IGN); // hup - ignore signal(SIGINT, signals); // ctrl+C - quit @@ -159,10 +188,39 @@ int main(int argc, char **argv){ pthread_t dthr; logmnt(fcoords, NULL); if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); - // goto 10 degr with 2d/s and try to track for 8 seconds - move(10., 10.+2./60., 2.); - // be sure to move @ 0,0 - Mount.moveTo(&M.X, &M.Y); + // move to 10/10 + coordpair_t coords, speeds, adders, tadd; + coords = (coordpair_t){.X = 10., .Y = 20.}; + speeds = (coordpair_t){.X = 1., .Y = 2.}; + adders = (coordpair_t){.X = 0.01, .Y = 0.01}; + tadd = (coordpair_t){.X = 1., .Y = 2.}; + green("Goto\n"); + if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go"); + DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y); + green("Waiting X==4\n"); + Wait(4., 1); + // now we are at point by Y but still moving by X; make small correction by X/Y into '+' + green("Mkcorr 1\n"); + if(MCC_E_OK != mkcorr(&adders, &tadd)) ERRX("Can't make corr"); + green("Waiting X==6\n"); + Wait(6., 1); + green("Goto more\n"); + coords = (coordpair_t){.X = 20., .Y = 30.}; + if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go"); + DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y); + green("Waiting Y==14\n"); + Wait(14., 0); + // now we are @ point, make the same small correction again + green("Mkcorr 2\n"); + if(MCC_E_OK != mkcorr(&coords, &speeds)) ERRX("Can't make corr"); + // wait for 5 seconds + green("Wait for 5 seconds\n"); + sleep(5); + // return to zero and wait + green("Return 2 zero and wait\n"); + return2zero(); + Wait(0., 0); + Wait(0., 1); // wait moving ends pthread_join(dthr, NULL); signals(0); diff --git a/Auxiliary_utils/LibSidServo/libsidservo.creator.user b/Auxiliary_utils/LibSidServo/libsidservo.creator.user index 1b41ef1..a298b58 100644 --- a/Auxiliary_utils/LibSidServo/libsidservo.creator.user +++ b/Auxiliary_utils/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/Auxiliary_utils/LibSidServo/main.c b/Auxiliary_utils/LibSidServo/main.c index 2a80709..f9d6518 100644 --- a/Auxiliary_utils/LibSidServo/main.c +++ b/Auxiliary_utils/LibSidServo/main.c @@ -202,13 +202,6 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){ s.YBits = cmd->YBits; DBG("X->%d, Y->%d, Xs->%d, Ys->%d", s.Xmot, s.Ymot, s.Xspeed, s.Yspeed); if(!cmdS(&s)) return MCC_E_FAILED; - cmd->Xmot = X_MOT2RAD(s.Xmot); - cmd->Ymot = Y_MOT2RAD(s.Ymot); - cmd->Xspeed = X_MOTSPD2RS(s.Xspeed); - cmd->Yspeed = Y_MOTSPD2RS(s.Yspeed); - cmd->xychange = s.xychange; - cmd->XBits = s.XBits; - cmd->YBits = s.YBits; return MCC_E_OK; } @@ -229,14 +222,6 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){ l.Xatime = S2ADDER(cmd->Xatime); l.Yatime = S2ADDER(cmd->Yatime); if(!cmdL(&l)) return MCC_E_FAILED; - cmd->Xmot = X_MOT2RAD(l.Xmot); - cmd->Ymot = Y_MOT2RAD(l.Ymot); - cmd->Xspeed = X_MOTSPD2RS(l.Xspeed); - cmd->Yspeed = Y_MOTSPD2RS(l.Yspeed); - cmd->Xadder = X_MOTSPD2RS(l.Xadder); - cmd->Yadder = Y_MOTSPD2RS(l.Yadder); - cmd->Xatime = ADDER2S(l.Xatime); - cmd->Yatime = ADDER2S(l.Yatime); return MCC_E_OK; } diff --git a/Auxiliary_utils/LibSidServo/serial.c b/Auxiliary_utils/LibSidServo/serial.c index 62fa57b..5bebab9 100644 --- a/Auxiliary_utils/LibSidServo/serial.c +++ b/Auxiliary_utils/LibSidServo/serial.c @@ -541,6 +541,8 @@ static int wr(const data_t *out, data_t *in, int needeol){ if(b < 0) break; // nothing to read -> go out in->buf[in->len++] = (uint8_t) b; } + //DBG("Clear trashing input"); + while(getmntbyte() > -1); return TRUE; } @@ -606,11 +608,7 @@ static int bincmd(uint8_t *cmd, int len){ data_t d; d.buf = cmd; d.len = d.maxlen = len; - ret = wr(&d, &d, 0); -#ifdef EBUG - if(len == sizeof(SSscmd)) logscmd((SSscmd*)cmd); - else loglcmd((SSlcmd*)cmd); -#endif + ret = wr(&d, NULL, 0); DBG("%s", ret ? "SUCCESS" : "FAIL"); rtn: pthread_mutex_unlock(&mntmutex); diff --git a/Auxiliary_utils/LibSidServo/ssii.h b/Auxiliary_utils/LibSidServo/ssii.h index 8ea4e5f..ae8ff2f 100644 --- a/Auxiliary_utils/LibSidServo/ssii.h +++ b/Auxiliary_utils/LibSidServo/ssii.h @@ -168,11 +168,15 @@ // Loop freq #define SITECH_LOOP_FREQUENCY (1953.) -// steps per revolution +// steps per revolution (SSI - x4 - for SSI) // 13312000 / 4 = 3328000 -#define X_MOT_STEPSPERREV (3325952.) +#define X_MOT_STEPSPERREV_SSI (13312000.) +//#define X_MOT_STEPSPERREV (3325952.) +#define X_MOT_STEPSPERREV (3328000.) // 17578668 / 4 = 4394667 -#define Y_MOT_STEPSPERREV (4394960.) +#define Y_MOT_STEPSPERREV_SSI (17578668.) +//#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) @@ -195,8 +199,8 @@ #define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY)) // adder time to seconds vice versa -#define ADDER2S(a) ((a) * SITECH_LOOP_FREQUENCY) -#define S2ADDER(s) ((s) / SITECH_LOOP_FREQUENCY) +#define ADDER2S(a) ((a) / SITECH_LOOP_FREQUENCY) +#define S2ADDER(s) ((s) * SITECH_LOOP_FREQUENCY) // encoder per revolution #define X_ENC_STEPSPERREV (67108864.)