timur/ASIBFM700
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

LibSidServo: fix velocity calculation errors in real mode

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2026-03-11 14:06:58 +03:00
parent b44e695e1c
commit a35b62d21d
4 changed files with 151 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
LibSidServo/main.c
View File

@@ -47,10 +47,10 @@ static movemodel_t *Xmodel, *Ymodel;
// accelerations: xa=12.6 deg/s^2, ya= 9.5 deg/s^2
limits_t
Xlimits = {
.min = {.coord = -3.1241, .speed = -1e-10, .accel = 1e-6},
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
.max = {.coord = 3.1241, .speed = 0.174533, .accel = 0.219911}},
Ylimits = {
.min = {.coord = -3.1241, .speed = -1e-10, .accel = 1e-6},
.min = {.coord = -3.1241, .speed = 1e-10, .accel = 1e-6},
.max = {.coord = 3.1241, .speed = 0.139626, .accel = 0.165806}}
;
static mcc_errcodes_t shortcmd(short_command_t *cmd);
@@ -93,7 +93,7 @@ static int initstarttime(){
// return difference (in seconds) between time1 and time0
double timediff(const struct timespec *time1, const struct timespec *time0){
if(!time1 || !time0) return -1.;
return (double)(time1->tv_sec - time0->tv_sec) + (double)(time1->tv_nsec - time0->tv_nsec) / 1e9;
return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / 1e9;
}
// difference between given time and last initstarttime() call
double timediff0(const struct timespec *time1){
@@ -161,6 +161,8 @@ 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];
/*DBG("old: x=%g, t=%g, t2=%g, xt=%g; sum: %g, t=%g, t2=%g, xt=%g", oldx, oldt, oldt2, oldxt,
l->xsum, l->tsum, l->t2sum, l->xtsum);*/
double t2 = t * t, xt = x * t;
l->x[idx] = x; l->t2[idx] = t2;
l->t[idx] = t; l->xt[idx] = xt;
@@ -172,9 +174,9 @@ double LS_calc_slope(less_square_t *l, double x, double t){
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;
//DBG("x=%g, t=%g; idx=%zd, arrsz=%zd, den=%g; xsum=%g, num=%g", x, t, l->idx, l->arraysz, denominator, l->xsum, numerator);
// point: (sum_x - slope * sum_t) / n;
return (numerator / denominator);
}
@@ -319,8 +321,8 @@ static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){
*/
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(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
// updateMotorPos() here can make a problem; TODO: remove?
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};