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

last week

Browse Source
This commit is contained in:
Edward Emelianov
2025-08-04 09:02:22 +03:00
parent 25438960e6
commit 219cec6055
5 changed files with 88 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
LibSidServo/ramp.c
View File

@@ -94,11 +94,6 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
double dt0s = curspeed / m->Max.accel; // time of stopping phase
double dx0s = curspeed * dt0s / 2.; // distance
DBG("dt0s=%g, dx0s=%g, curspeed=%g", dt0s, dx0s, curspeed);
// TODO: fix this! Target should stop and after thar reach given coordinate!!!
if(dx0s - Dx > coord_tolerance){
DBG("distance too short");
goto ret;
}
if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
DBG("Distance good to just stop");
pthread_mutex_unlock(&m->mutex);
@@ -106,17 +101,58 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
ret = TRUE;
goto ret;
}
if(m->curparams.speed * sign < 0. || m->state == ST_STOP){ // we should change speed sign
// after stop we will have full profile
double dxs3 = Dx - dx0s;
double newspeed = sqrt(m->Max.accel * dxs3);
if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed
DBG("dxs3=%g, setspeed=%g", dxs3, setspeed);
dt01 = fabs(sign*setspeed - m->curparams.speed) / m->Max.accel;
m->Params[0].accel = sign * m->Max.accel;
if(m->state == ST_STOP) dx01 = setspeed * dt01 / 2.;
else dx01 = dt01 * (dt01 / 2. * m->Max.accel - curspeed);
DBG("dx01=%g, dt01=%g", dx01, dt01);
if(m->curparams.speed * sign < 0. || m->state == ST_STOP || (dx0s > Dx + coord_tolerance)){ // we should change speed sign
DBG("SIGN of speed should be changed!");
double sign_current = (m->curparams.speed >= 0.) ? 1. : -1.;
if (m->state == ST_STOP) {
// Already stopped
dx0s = 0.;
sign_current = 0.;
}
double stop_coord = m->curparams.coord + sign_current * dx0s;
double Dx_after = fabs(x->coord - stop_coord);
double sign_after = (x->coord > stop_coord) ? 1. : -1.;
// Calculate new max speed for reverse movement
double setspeed = sqrt(m->Max.accel * Dx_after);
if (setspeed > x->speed) setspeed = x->speed;
if (setspeed > m->Max.speed) setspeed = m->Max.speed;
if (setspeed < m->Min.speed) {
DBG("New speed (%g) too small (<%g)", setspeed, m->Min.speed);
goto ret;
}
double t_acc = setspeed / m->Max.accel;
// Stage 0: Stop current movement (if moving)
m->Times[STAGE_ACCEL] = t;
m->Params[STAGE_ACCEL].coord = m->curparams.coord;
m->Params[STAGE_ACCEL].speed = m->curparams.speed;
if (m->state != ST_STOP) {
m->Params[STAGE_ACCEL].accel = -sign_current * m->Max.accel;
m->Times[STAGE_MAXSPEED] = t + dt0s;
} else {
m->Params[STAGE_ACCEL].accel = 0.;
m->Times[STAGE_MAXSPEED] = t;
}
// Stage 1: Accelerate in opposite direction
m->Params[STAGE_MAXSPEED].coord = stop_coord;
m->Params[STAGE_MAXSPEED].speed = 0.;
m->Params[STAGE_MAXSPEED].accel = sign_after * m->Max.accel;
m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + t_acc;
m->Params[STAGE_DECEL].coord = stop_coord + sign_after * (0.5 * m->Max.accel * t_acc * t_acc);
m->Params[STAGE_DECEL].speed = sign_after * setspeed;
m->Params[STAGE_DECEL].accel = -sign_after * m->Max.accel;
// Stage 2: Decelerate to stop at target
m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + t_acc;
m->Params[STAGE_STOPPED].coord = x->coord;
m->Params[STAGE_STOPPED].speed = 0.;
m->Params[STAGE_STOPPED].accel = 0.;
ret = TRUE;
goto ret;
}else{ // increase or decrease speed without stopping phase
dt01 = fabs(sign*setspeed - m->curparams.speed) / m->Max.accel;
double a = sign * m->Max.accel;
@@ -128,7 +164,7 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
if(dx01 + dx23 > Dx){ // calculate max speed
setspeed = sqrt(m->Max.accel * Dx - curspeed * curspeed / 2.);
if(setspeed < curspeed){
setspeed = m->curparams.speed;
setspeed = curspeed;
dt01 = 0.; dx01 = 0.;
m->Params[0].accel = 0.;
}else{
@@ -164,13 +200,15 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
p = &m->Params[STAGE_STOPPED];
p->accel = 0.; p->speed = 0.; p->coord = x->coord;
m->Times[STAGE_STOPPED] = m->Times[STAGE_DECEL] + dt23;
for(int i = 0; i < 4; ++i)
DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel);
m->state = ST_MOVE;
m->movingstage = STAGE_ACCEL;
ret = TRUE;
ret:
if(ret){
m->state = ST_MOVE;
m->movingstage = STAGE_ACCEL;
for(int i = 0; i < 4; ++i)
DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
m->Times[i], m->Params[i].coord, m->Params[i].speed, m->Params[i].accel);
}
pthread_mutex_unlock(&m->mutex);
return ret;
}