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

5
LibSidServo/examples/dumpswing.c
View File

@ -55,7 +55,7 @@ static sl_option_t cmdlnopts[] = {
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"},
{"period", NEED_ARG, NULL, 'p', arg_double, APTR(&G.period), "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
{"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"},
{"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reached if period is too small): [-45:45]deg (default: 5)"},
{"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"},
{"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"},
end_option
@ -114,7 +114,8 @@ int main(int argc, char **argv){
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
if(G.Ncycles < 7) ERRX("Ncycles should be >7");
if(G.amplitude < 0.01 || G.amplitude > 45.)
double absamp = fabs(G.amplitude);
if(absamp < 0.01 || absamp > 45.)
ERRX("Amplitude should be from 0.01 to 45 degrees");
if(G.period < 0.1 || G.period > 900.)
ERRX("Period should be from 0.1 to 900s");

4
LibSidServo/movingmodel.h
View File

@ -22,8 +22,8 @@
#include "sidservo.h"
// tolerance, time ticks
#define COORD_TOLERANCE_DEFAULT (1e-6)
#define COORD_TOLERANCE_MIN (1e-8)
#define COORD_TOLERANCE_DEFAULT (1e-8)
#define COORD_TOLERANCE_MIN (1e-12)
#define COORD_TOLERANCE_MAX (10.)
#define TIME_TICK_DEFAULT (0.0001)
#define TIME_TICK_MIN (1e-9)

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

5
LibSidServo/sidservo.h
View File

@ -32,6 +32,11 @@ extern "C"
#include <stdint.h>
#include <sys/time.h>
// acceptable position error - 0.1''
#define MCC_POSITION_ERROR (5e-7)
// acceptable disagreement between motor and axe encoders - 2''
#define MCC_ENCODERS_ERROR (1e-7)
// 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)

28
LibSidServo/ssii.c
View File

@ -189,16 +189,24 @@ mcc_errcodes_t updateMotorPos(){
continue;
}
DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
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, Dt=%g", nanotime() - t0);
return MCC_E_OK;
}
}else{
DBG("on position");
return MCC_E_OK;
mcc_errcodes_t OK = MCC_E_OK;
if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstatus == MNT_STOPPED){
DBG("NEED to sync X: motors=%g, axes=%g", md.motXposition.val, md.encXposition.val);
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
DBG("Xpos sync failed!");
OK = MCC_E_FAILED;
}else DBG("Xpos sync OK, Dt=%g", nanotime() - t0);
}
if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstatus == MNT_STOPPED){
DBG("NEED to sync Y: motors=%g, axes=%g", md.motYposition.val, md.encYposition.val);
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
DBG("Ypos sync failed!");
OK = MCC_E_FAILED;
}else DBG("Ypos sync OK, Dt=%g", nanotime() - t0);
}
if(MCC_E_OK == OK){
DBG("Encoders synced");
return OK;
}
}
DBG("NO DATA; dt = %g", t - t0);