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

less squares 4 speed + fixed some bugs (but found more)

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-07-31 17:03:15 +03:00
parent ca9dcdfa68
commit 9fbd858086
15 changed files with 428 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
LibSidServo/main.c
View File

@@ -37,10 +37,10 @@ static movemodel_t *Xmodel, *Ymodel;
// radians, rad/sec, rad/sec^2
static limits_t
Xlimits = {
.min = {.coord = -3.1241, .speed = 1e-8, .accel = 1e-6},
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
.max = {.coord = 3.1241, .speed = MCC_MAX_X_SPEED, .accel = MCC_X_ACCELERATION}},
Ylimits = {
.min = {.coord = -3.1241, .speed = 1e-8, .accel = 1e-6},
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
.max = {.coord = 3.1241, .speed = MCC_MAX_Y_SPEED, .accel = MCC_Y_ACCELERATION}}
;
static mcc_errcodes_t shortcmd(short_command_t *cmd);
@@ -76,22 +76,73 @@ static void quit(){
void getModData(mountdata_t *mountdata){
if(!mountdata || !Xmodel || !Ymodel) return;
static double oldmt = -100.; // old `millis measurement` time
static uint32_t oldmillis = 0;
double tnow = nanotime();
moveparam_t Xp, Yp;
movestate_t Xst = Xmodel->get_state(&Xp);
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(&Xp, tnow);
movestate_t Yst = Ymodel->get_state(&Yp);
if(Yst == ST_MOVE) Yst = Ymodel->proc_move(&Yp, tnow);
movestate_t Xst = Xmodel->get_state(Xmodel, &Xp);
//DBG("Xstate = %d", Xst);
if(Xst == ST_MOVE) Xst = Xmodel->proc_move(Xmodel, &Xp, tnow);
movestate_t Yst = Ymodel->get_state(Ymodel, &Yp);
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.val = mountdata->encXposition.val = Xp.coord;
mountdata->motYposition.val = mountdata->encYposition.val = Yp.coord;
mountdata->encXspeed.t = mountdata->encYspeed.t = tnow;
mountdata->encXspeed.val = Xp.speed;
mountdata->encYspeed.val = Yp.speed;
mountdata->millis = (uint32_t)(tnow * 1e3);
getXspeed(); getYspeed();
if(tnow - oldmt > Conf.MountReqInterval){
oldmillis = mountdata->millis = (uint32_t)(tnow * 1e3);
oldmt = tnow;
}else mountdata->millis = oldmillis;
}
/**
* less square calculations of speed
*/
less_square_t *LS_init(size_t Ndata){
if(Ndata < 5){
DBG("Ndata=%zd - TOO SMALL", Ndata);
return NULL;
}
DBG("Init less squares: %zd", Ndata);
less_square_t *l = calloc(1, sizeof(less_square_t));
l->x = calloc(Ndata, sizeof(double));
l->t2 = calloc(Ndata, sizeof(double));
l->t = calloc(Ndata, sizeof(double));
l->xt = calloc(Ndata, sizeof(double));
l->arraysz = Ndata;
return l;
}
void LS_delete(less_square_t **l){
if(!l || !*l) return;
free((*l)->x); free((*l)->t2); free((*l)->t); free((*l)->xt);
free(*l);
*l = NULL;
}
// add next data portion and calculate current slope
double LS_calc_slope(less_square_t *l, double x, double t){
if(!l) return 0.;
size_t idx = l->idx;
double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
double t2 = t * t, xt = x * t;
l->x[idx] = x; l->t2[idx] = t2;
l->t[idx] = t; l->xt[idx] = xt;
++idx;
l->idx = (idx >= l->arraysz) ? 0 : idx;
l->xsum += x - oldx;
l->t2sum += t2 - oldt2;
l->tsum += t - oldt;
l->xtsum += xt - oldxt;
double n = (double)l->arraysz;
double denominator = n * l->t2sum - l->tsum * l->tsum;
//DBG("idx=%zd, arrsz=%zd, den=%g", l->idx, l->arraysz, denominator);
if(fabs(denominator) < 1e-7) return 0.;
double numerator = n * l->xtsum - l->xsum * l->tsum;
// point: (sum_x - slope * sum_t) / n;
return (numerator / denominator);
}
/**
* @brief init - open serial devices and do other job
* @param c - initial configuration
@@ -105,7 +156,7 @@ static mcc_errcodes_t init(conf_t *c){
Xmodel = model_init(&Xlimits);
Ymodel = model_init(&Ylimits);
if(Conf.RunModel){
if(!Xmodel || !Ymodel) return MCC_E_FAILED;
if(!Xmodel || !Ymodel || !openMount()) return MCC_E_FAILED;
return MCC_E_OK;
}
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
@@ -132,10 +183,9 @@ static mcc_errcodes_t init(conf_t *c){
DBG("Wrong speed interval");
ret = MCC_E_BADFORMAT;
}
uint8_t buf[1024];
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;
//uint8_t buf[1024];
//data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
if(!SSrawcmd(CMD_EXITACM, NULL)) ret = MCC_E_FAILED;
if(ret != MCC_E_OK) return ret;
return updateMotorPos();
}
@@ -180,14 +230,6 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
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;
if(Conf.RunModel){
double curt = nanotime();
moveparam_t param = {0};
param.coord = target->X; param.speed = MCC_MAX_X_SPEED;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = target->Y; param.speed = MCC_MAX_Y_SPEED;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
}
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
DBG("x,y: %g, %g", target->X, target->Y);
@@ -227,14 +269,14 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
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;
if(Conf.RunModel){
/* if(Conf.RunModel){
double curt = nanotime();
moveparam_t param = {0};
param.coord = target->X; param.speed = speed->X;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = target->Y; param.speed = speed->Y;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
}
}*/
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
cmd.Xmot = target->X;
@@ -254,8 +296,8 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
static mcc_errcodes_t emstop(){
if(Conf.RunModel){
double curt = nanotime();
Xmodel->emergency_stop(curt);
Ymodel->emergency_stop(curt);
Xmodel->emergency_stop(Xmodel, curt);
Ymodel->emergency_stop(Ymodel, curt);
return MCC_E_OK;
}
if(!SSstop(TRUE)) return MCC_E_FAILED;
@@ -265,8 +307,8 @@ static mcc_errcodes_t emstop(){
static mcc_errcodes_t stop(){
if(Conf.RunModel){
double curt = nanotime();
Xmodel->stop(curt);
Ymodel->stop(curt);
Xmodel->stop(Xmodel, curt);
Ymodel->stop(Ymodel,curt);
return MCC_E_OK;
}
if(!SSstop(FALSE)) return MCC_E_FAILED;
@@ -280,7 +322,15 @@ static mcc_errcodes_t stop(){
*/
static mcc_errcodes_t shortcmd(short_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT;
if(Conf.RunModel) return MCC_E_FAILED;
if(Conf.RunModel){
double curt = nanotime();
moveparam_t param = {0};
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
return MCC_E_OK;
}
SSscmd s = {0};
DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
s.Xmot = X_RAD2MOT(cmd->Xmot);
@@ -302,7 +352,15 @@ static mcc_errcodes_t shortcmd(short_command_t *cmd){
*/
static mcc_errcodes_t longcmd(long_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT;
if(Conf.RunModel) return MCC_E_FAILED;
if(Conf.RunModel){
double curt = nanotime();
moveparam_t param = {0};
param.coord = cmd->Xmot; param.speed = cmd->Xspeed;
if(!model_move2(Xmodel, &param, curt)) return MCC_E_FAILED;
param.coord = cmd->Ymot; param.speed = cmd->Yspeed;
if(!model_move2(Ymodel, &param, curt)) return MCC_E_FAILED;
return MCC_E_OK;
}
SSlcmd l = {0};
l.Xmot = X_RAD2MOT(cmd->Xmot);
l.Ymot = Y_RAD2MOT(cmd->Ymot);
@@ -455,3 +513,4 @@ mount_t Mount = {
.saveHWconfig = write_hwconf,
.currentT = nanotime,
};