eddy/mountcontrol
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

..

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-07-24 15:31:40 +03:00
parent 133863e47f
commit 8f36f89d7a
10 changed files with 102 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
LibSidServo/examples/dump.c
View File

@ -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");
}

7
LibSidServo/examples/dumpmoving.c
View File

@ -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;

3
LibSidServo/examples/dumpmoving_scmd.c
View File

@ -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);

11
LibSidServo/examples/dumpswing.c
View File

@ -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

41
LibSidServo/examples/goto.c
View File

@ -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);

2
LibSidServo/libsidservo.config
View File

@ -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"

97
LibSidServo/main.c
View File

@ -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);

4
LibSidServo/serial.c
View File

@ -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;

6
LibSidServo/sidservo.h
View File

@ -27,6 +27,10 @@ extern "C"
#include <stdint.h>
#include <sys/time.h>
// 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

14
LibSidServo/ssii.h
View File

@ -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))