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
Download Patch File Download Diff File Expand all files Collapse all files

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