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

1
LibSidServo/examples/conf.c
View File

@ -44,6 +44,7 @@ static sl_option_t opts[] = {
{"EncoderXDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderXDevPath), "path to X encoder (/dev/encoderX0)"},
{"EncoderYDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderYDevPath), "path to Y encoder (/dev/encoderY0)"},
{"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
{"RunModel", NEED_ARG, NULL, 0, arg_int, APTR(&Config.RunModel), "instead of real hardware run emulation"},
end_option
};

3
LibSidServo/examples/dump.c
View File

@ -115,7 +115,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
enct = tmsr;
if(fcoords) logmnt(fcoords, &mdata);
if(mdata.millis == mdmillis) continue;
//DBG("ctr=%d", ctr);
//DBG("ctr=%d, motpos=%g/%g", ctr, mdata.motXposition.val, mdata.motYposition.val);
mdmillis = mdata.millis;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
xlast = mdata.motXposition.val;
@ -123,6 +123,7 @@ void dumpmoving(FILE *fcoords, double t, int N){
ctr = 0;
}else ++ctr;
}
DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.currentT() - t0);
}
/**

26
LibSidServo/examples/dumpmoving_dragNtrack.c
View File

@ -120,16 +120,13 @@ static coordpair_t lastTag = {0}, lastSpeed = {0};
// slew to given position and start tracking
// pos/speed in deg and deg/s
static mcc_errcodes_t gotos(coordpair_t *target, coordpair_t *speed){
static mcc_errcodes_t gotos(const coordpair_t *target, const coordpair_t *speed){
short_command_t cmd = {0};
DBG("Try to move to (%g, %g) with speed (%g, %g)",
target->X, target->Y, speed->X, speed->Y);
target->X = DEG2RAD(target->X);
target->Y = DEG2RAD(target->Y);
speed->X = DEG2RAD(speed->X);
speed->Y = DEG2RAD(speed->Y);
cmd.Xmot = target->X; cmd.Ymot = target->Y;
cmd.Xspeed = speed->X; cmd.Yspeed = speed->Y;
cmd.Xmot = DEG2RAD(target->X); cmd.Ymot = DEG2RAD(target->Y);
cmd.Xspeed = DEG2RAD(speed->X);
cmd.Yspeed = DEG2RAD(speed->Y);
lastTag = *target;
lastSpeed = *speed;
/*cmd.xychange = 1;
@ -142,7 +139,8 @@ static mcc_errcodes_t return2zero(){
short_command_t cmd = {0};
DBG("Try to move to zero");
cmd.Xmot = 0.; cmd.Ymot = 0.;
cmd.Xspeed = DEG2RAD(10.); cmd.Yspeed = DEG2RAD(10.);
cmd.Xspeed = MCC_MAX_X_SPEED;
cmd.Yspeed = MCC_MAX_Y_SPEED;
/*cmd.xychange = 1;
cmd.XBits = 100;
cmd.YBits = 20;*/
@ -151,11 +149,11 @@ static mcc_errcodes_t return2zero(){
static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){
long_command_t cmd = {0};
cmd.Xspeed = lastSpeed.X;
cmd.Yspeed = lastSpeed.Y;
cmd.Xmot = lastTag.X;
cmd.Ymot = lastTag.Y;
cmd.Xadder = adder->X; cmd.Yadder = adder->Y;
cmd.Xspeed = DEG2RAD(lastSpeed.X);
cmd.Yspeed = DEG2RAD(lastSpeed.Y);
cmd.Xmot = DEG2RAD(lastTag.X);
cmd.Ymot = DEG2RAD(lastTag.Y);
cmd.Xadder = DEG2RAD(adder->X); cmd.Yadder = DEG2RAD(adder->Y);
cmd.Xatime = time->X; cmd.Yatime = time->Y;
return Mount.longCmd(&cmd);
}
@ -218,7 +216,7 @@ int main(int argc, char **argv){
sleep(5);
// return to zero and wait
green("Return 2 zero and wait\n");
return2zero();
if(!return2zero()) ERRX("Can't return");
Wait(0., 0);
Wait(0., 1);
// wait moving ends

12
LibSidServo/examples/dumpswing.c
View File

@ -89,12 +89,12 @@ void waithalf(double t){
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
//DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
ctr = 0;
}else{
DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
//DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
++ctr;
}
}
@ -162,9 +162,13 @@ int main(int argc, char **argv){
DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", Mount.currentT()-t0);
}
tag = (coordpair_t){.X = 0., .Y = 0.};
green("Move to zero @ %g\n", Mount.currentT());
tag = (coordpair_t){0};
// be sure to move @ 0,0
Mount.moveTo(&tag);
if(MCC_E_OK != Mount.moveTo(&tag)){
Mount.emergStop();
Mount.moveTo(&tag);
}
// wait moving ends
pthread_join(dthr, NULL);
#undef SCMD

5
LibSidServo/libsidservo.config
View File

@ -1,6 +1,7 @@
// Add predefined macros for your project here. For example:
// #define THE_ANSWER 42
#define EBUG
#define _POSIX_C_SOURCE 111
#define _POSIX_C_SOURCE 11111111
#define PACKAGE_VERSION "0.0.1"
#define _XOPEN_SOURCE 666
#define _DEFAULT_SOURCE

40
LibSidServo/libsidservo.creator.user
View File

@ -1,10 +1,10 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 17.0.0, 2025-07-29T22:07:00. -->
<!-- Written by QtCreator 17.0.0, 2025-07-30T17:30:52. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
@ -40,9 +40,9 @@
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">false</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
@ -51,10 +51,10 @@
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">true</value>
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
</valuemap>
@ -75,13 +75,11 @@
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
<value type="int" key="AutoTest.RunAfterBuild">0</value>
<value type="bool" key="AutoTest.UseGlobal">true</value>
<valuelist type="QVariantList" key="ClangCodeModel.CustomCommandLineKey"/>
<value type="bool" key="ClangCodeModel.UseGlobalConfig">true</value>
<valuemap type="QVariantMap" key="ClangTools">
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
<value type="int" key="ClangTools.ParallelJobs">8</value>
<value type="int" key="ClangTools.ParallelJobs">4</value>
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
@ -97,12 +95,12 @@
<value type="bool" key="HasPerBcDcs">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/C-sources/erfa</value>
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/Docs/SAO/10micron/C-sources/erfa_functions</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
@ -112,8 +110,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Сборка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
@ -125,8 +123,8 @@
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Очистка</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
@ -141,8 +139,8 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
@ -154,7 +152,9 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
@ -173,8 +173,8 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
@ -186,7 +186,9 @@
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>

6
LibSidServo/libsidservo.files
View File

@ -1,5 +1,4 @@
CMakeLists.txt
dbg.h
examples/SSIIconf.c
examples/conf.c
examples/conf.h
@ -18,6 +17,11 @@ serial.c
examples/CMakeLists.txt
examples/traectories.c
examples/traectories.h
main.h
movingmodel.c
movingmodel.h
ramp.c
ramp.h
serial.h
ssii.c
ssii.h

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

10
LibSidServo/main.h
View File

@ -29,6 +29,16 @@
extern conf_t Conf;
double nanotime();
void getModData(mountdata_t *mountdata);
typedef struct{
double *x, *t, *t2, *xt; // arrays of coord/time and multiply
double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
size_t idx; // index of current data in array
size_t arraysz; // size of arrays
} less_square_t;
less_square_t *LS_init(size_t Ndata);
void LS_delete(less_square_t **ls);
double LS_calc_slope(less_square_t *l, double x, double t);
// unused arguments of functions
#define _U_ __attribute__((__unused__))

18
LibSidServo/movingmodel.c
View File

@ -36,9 +36,8 @@ static void chkminmax(double *min, double *max){
movemodel_t *model_init(limits_t *l){
if(!l) return FALSE;
movemodel_t *model = calloc(1, sizeof(movemodel_t));
memcpy(model, &trapez, sizeof(movemodel_t));
if(!model->init_limits) goto fail;
movemodel_t *m = calloc(1, sizeof(movemodel_t));
*m = trapez;
moveparam_t *max = &l->max, *min = &l->min;
if(min->speed < 0.) min->speed = -min->speed;
if(max->speed < 0.) max->speed = -max->speed;
@ -47,10 +46,13 @@ movemodel_t *model_init(limits_t *l){
chkminmax(&min->coord, &max->coord);
chkminmax(&min->speed, &max->speed);
chkminmax(&min->accel, &max->accel);
if(!model->init_limits(l)) return NULL;
return model;
fail:
free(model); return NULL;
m->Min = l->min;
m->Max = l->max;
m->movingstage = STAGE_STOPPED;
m->state = ST_STOP;
pthread_mutex_init(&m->mutex, NULL);
DBG("model inited");
return m;
}
int model_move2(movemodel_t *model, moveparam_t *target, double t){
@ -59,5 +61,5 @@ int model_move2(movemodel_t *model, moveparam_t *target, double t){
// only positive velocity
if(target->speed < 0.) target->speed = -target->speed;
// don't mind about acceleration - user cannot set it now
return model->calculate(target, t);
return model->calculate(model, target, t);
}

36
LibSidServo/movingmodel.h
View File

@ -17,12 +17,13 @@
*/
#pragma once
#include <pthread.h>
#include "sidservo.h"
// tolerance, time ticks
#define COORD_TOLERANCE_DEFAULT (0.01)
#define COORD_TOLERANCE_MIN (0.0001)
#define COORD_TOLERANCE_DEFAULT (1e-6)
#define COORD_TOLERANCE_MIN (1e-8)
#define COORD_TOLERANCE_MAX (10.)
#define TIME_TICK_DEFAULT (0.0001)
#define TIME_TICK_MIN (1e-9)
@ -46,14 +47,29 @@ typedef struct{
double acceleration;
} limits_t;
typedef struct{
int (*init_limits)(limits_t *lim); // init values of limits, jerk
int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t
movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t
movestate_t (*get_state)(moveparam_t *cur); // get current moving state
void (*stop)(double t); // stop by ramp
void (*emergency_stop)(double t); // stop with highest acceleration
double (*stoppenanotime)(); // time when moving will ends
typedef enum{
STAGE_ACCEL, // start from zero speed and accelerate to Max speed
STAGE_MAXSPEED, // go with target speed
STAGE_DECEL, // go from target speed to zero
STAGE_STOPPED, // stop
STAGE_AMOUNT
} movingstage_t;
typedef struct movemodel{
moveparam_t Min;
moveparam_t Max;
movingstage_t movingstage;
movestate_t state;
double Times[STAGE_AMOUNT];
moveparam_t Params[STAGE_AMOUNT];
moveparam_t curparams; // init values of limits, jerk
int (*calculate)(struct movemodel *m, moveparam_t *target, double t); // calculate stages of traectory beginning from t
movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t); // calculate next model point for time t
movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur); // get current moving state
void (*stop)(struct movemodel *m, double t); // stop by ramp
void (*emergency_stop)(struct movemodel *m, double t); // stop with highest acceleration
double (*stoppedtime)(struct movemodel *m); // time when moving will ends
pthread_mutex_t mutex;
} movemodel_t;
movemodel_t *model_init(limits_t *l);

242
LibSidServo/ramp.c
View File

@ -25,53 +25,38 @@
#include "ramp.h"
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
static movestate_t state = ST_STOP;
static moveparam_t Min, Max;
typedef enum{
STAGE_ACCEL, // start from zero speed and accelerate to Max speed
STAGE_MAXSPEED, // go with target speed
STAGE_DECEL, // go from target speed to zero
STAGE_STOPPED, // stop
STAGE_AMOUNT
} movingstage_t;
static movingstage_t movingstage = STAGE_STOPPED;
static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts
static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage
static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
static int initlims(limits_t *lim){
if(!lim) return FALSE;
Min = lim->min;
Max = lim->max;
return TRUE;
static void emstop(movemodel_t *m, double _U_ t){
FNAME();
pthread_mutex_lock(&m->mutex);
m->curparams.accel = 0.;
m->curparams.speed = 0.;
bzero(m->Times, sizeof(double) * STAGE_AMOUNT);
bzero(m->Params, sizeof(moveparam_t) * STAGE_AMOUNT);
m->state = ST_STOP;
m->movingstage = STAGE_STOPPED;
pthread_mutex_unlock(&m->mutex);
}
static void emstop(double _U_ t){
curparams.accel = 0.;
curparams.speed = 0.;
bzero(Times, sizeof(Times));
bzero(Params, sizeof(Params));
state = ST_STOP;
movingstage = STAGE_STOPPED;
}
static void stop(double t){
if(state == ST_STOP || movingstage == STAGE_STOPPED) return;
movingstage = STAGE_DECEL;
state = ST_MOVE;
Times[STAGE_DECEL] = t;
Params[STAGE_DECEL].speed = curparams.speed;
if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel;
else Params[STAGE_DECEL].accel = Max.accel;
Params[STAGE_DECEL].coord = curparams.coord;
static void stop(movemodel_t *m, double t){
FNAME();
pthread_mutex_lock(&m->mutex);
if(m->state == ST_STOP || m->movingstage == STAGE_STOPPED) goto ret;
m->movingstage = STAGE_DECEL;
m->state = ST_MOVE;
m->Times[STAGE_DECEL] = t;
m->Params[STAGE_DECEL].speed = m->curparams.speed;
if(m->curparams.speed > 0.) m->Params[STAGE_DECEL].accel = -m->Max.accel;
else m->Params[STAGE_DECEL].accel = m->Max.accel;
m->Params[STAGE_DECEL].coord = m->curparams.coord;
// speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel;
m->Times[STAGE_STOPPED] = t - m->curparams.speed / m->Params[STAGE_DECEL].accel;
// coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
double dt = Times[STAGE_STOPPED] - t;
Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt +
Params[STAGE_DECEL].accel * dt * dt / 2.;
double dt = m->Times[STAGE_STOPPED] - t;
m->Params[STAGE_STOPPED].coord = m->curparams.coord + m->curparams.speed * dt +
m->Params[STAGE_DECEL].accel * dt * dt / 2.;
ret:
pthread_mutex_unlock(&m->mutex);
}
/**
@ -80,141 +65,166 @@ static void stop(double t){
* @param t - current time value
* @return FALSE if can't move with given parameters
*/
static int calc(moveparam_t *x, double t){
static int calc(movemodel_t *m, moveparam_t *x, double t){
DBG("coord/speed: %g/%g", x->coord, x->speed);
if(!x) return FALSE;
if(x->coord < Min.coord || x->coord > Max.coord) return FALSE;
if(x->speed < Min.speed || x->speed > Max.speed) return FALSE;
double Dx = fabs(x->coord - curparams.coord); // full distance
double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
pthread_mutex_lock(&m->mutex);
int ret = FALSE;
if(x->coord < m->Min.coord || x->coord > m->Max.coord){
DBG("Wrong coordinage [%g, %g]", m->Min.coord, m->Max.coord);
goto ret;
}
if(x->speed < m->Min.speed || x->speed > m->Max.speed){
DBG("Wrong speed [%g, %g]", m->Min.speed, m->Max.speed);
goto ret;
}
double Dx = fabs(x->coord - m->curparams.coord); // full distance
double sign = (x->coord > m->curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
// we have two variants: with or without stage with constant speed
double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed
double dt23 = x->speed / m->Max.accel; // time of deceleration stage for given speed
double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs)
DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23);
double setspeed = x->speed; // new max speed (we can change it if need)
double dt01, dx01; // we'll fill them depending on starting conditions
Times[0] = t;
Params[0].speed = curparams.speed;
Params[0].coord = curparams.coord;
m->Times[0] = t;
m->Params[0].speed = m->curparams.speed;
m->Params[0].coord = m->curparams.coord;
double curspeed = fabs(curparams.speed);
double dt0s = curspeed / Max.accel; // time of stopping phase
double curspeed = fabs(m->curparams.speed);
double dt0s = curspeed / m->Max.accel; // time of stopping phase
double dx0s = curspeed * dt0s / 2.; // distance
DBG("dt0s=%g, dx0s=%g", dt0s, dx0s);
if(dx0s > Dx){
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");
return FALSE;
goto ret;
}
if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
DBG("Distance good to just stop");
stop(t);
return TRUE;
pthread_mutex_unlock(&m->mutex);
stop(m, t);
ret = TRUE;
goto ret;
}
if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign
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(Max.accel * dxs3);
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 - curparams.speed) / Max.accel;
Params[0].accel = sign * Max.accel;
if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
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);
}else{ // increase or decrease speed without stopping phase
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
double a = sign * Max.accel;
if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
dt01 = fabs(sign*setspeed - m->curparams.speed) / m->Max.accel;
double a = sign * m->Max.accel;
if(sign * m->curparams.speed < 0.){DBG("change direction"); a = -a;}
else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;}
//double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.;
dx01 = sign*(curspeed * dt01 + a * dt01 * dt01 / 2.);
DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01);
if(dx01 + dx23 > Dx){ // calculate max speed
setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.);
setspeed = sqrt(m->Max.accel * Dx - curspeed * curspeed / 2.);
if(setspeed < curspeed){
setspeed = curparams.speed;
setspeed = m->curparams.speed;
dt01 = 0.; dx01 = 0.;
Params[0].accel = 0.;
m->Params[0].accel = 0.;
}else{
Params[0].accel = a;
dt01 = fabs(setspeed - curspeed) / Max.accel;
dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.;
m->Params[0].accel = a;
dt01 = fabs(setspeed - curspeed) / m->Max.accel;
dx01 = curspeed * dt01 + m->Max.accel * dt01 * dt01 / 2.;
}
}else Params[0].accel = a;
}else m->Params[0].accel = a;
}
if(setspeed < Min.speed){
DBG("New speed should be too small");
return FALSE;
if(setspeed < m->Min.speed){
DBG("New speed (%g) should be too small (<%g)", setspeed, m->Min.speed);
goto ret;
}
moveparam_t *p = &Params[STAGE_MAXSPEED];
moveparam_t *p = &m->Params[STAGE_MAXSPEED];
p->accel = 0.; p->speed = sign * setspeed;
p->coord = curparams.coord + dx01 * sign;
Times[STAGE_MAXSPEED] = Times[0] + dt01;
dt23 = setspeed / Max.accel;
p->coord = m->curparams.coord + dx01 * sign;
m->Times[STAGE_MAXSPEED] = m->Times[0] + dt01;
dt23 = setspeed / m->Max.accel;
dx23 = setspeed * dt23 / 2.;
DBG("setspeed=%g, dt23=%g, tx23=%g", setspeed, dt23, dx23);
// calculate dx12 and dt12
double dx12 = Dx - dx01 - dx23;
if(dx12 < -coord_tolerance){
DBG("Oops, WTF dx12=%g?", dx12);
return FALSE;
goto ret;
}
double dt12 = dx12 / setspeed;
p = &Params[STAGE_DECEL];
p->accel = -sign * Max.accel;
p = &m->Params[STAGE_DECEL];
p->accel = -sign * m->Max.accel;
p->speed = sign * setspeed;
p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12;
p = &Params[STAGE_STOPPED];
p->coord = m->Params[STAGE_MAXSPEED].coord + sign * dx12;
m->Times[STAGE_DECEL] = m->Times[STAGE_MAXSPEED] + dt12;
p = &m->Params[STAGE_STOPPED];
p->accel = 0.; p->speed = 0.; p->coord = x->coord;
Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23;
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,
Times[i], Params[i].coord, Params[i].speed, Params[i].accel);
state = ST_MOVE;
movingstage = STAGE_ACCEL;
return TRUE;
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:
pthread_mutex_unlock(&m->mutex);
return ret;
}
static movestate_t proc(moveparam_t *next, double t){
if(state == ST_STOP) goto ret;
static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
pthread_mutex_lock(&m->mutex);
if(m->state == ST_STOP) goto ret;
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
if(Times[s] <= t){ // check time for current stage
movingstage = s;
if(m->Times[s] <= t){ // check time for current stage
m->movingstage = s;
break;
}
}
if(movingstage == STAGE_STOPPED){
curparams.coord = Params[STAGE_STOPPED].coord;
emstop(t);
if(m->movingstage == STAGE_STOPPED){
m->curparams.coord = m->Params[STAGE_STOPPED].coord;
pthread_mutex_unlock(&m->mutex);
emstop(m, t);
goto ret;
}
// calculate current parameters
double dt = t - Times[movingstage];
double a = Params[movingstage].accel;
double v0 = Params[movingstage].speed;
double x0 = Params[movingstage].coord;
curparams.accel = a;
curparams.speed = v0 + a * dt;
curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
double dt = t - m->Times[m->movingstage];
double a = m->Params[m->movingstage].accel;
double v0 = m->Params[m->movingstage].speed;
double x0 = m->Params[m->movingstage].coord;
m->curparams.accel = a;
m->curparams.speed = v0 + a * dt;
m->curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
ret:
if(next) *next = curparams;
return state;
if(next) *next = m->curparams;
movestate_t st = m->state;
pthread_mutex_unlock(&m->mutex);
return st;
}
static movestate_t getst(moveparam_t *cur){
if(cur) *cur = curparams;
return state;
static movestate_t getst(movemodel_t *m, moveparam_t *cur){
pthread_mutex_lock(&m->mutex);
if(cur) *cur = m->curparams;
movestate_t st = m->state;
pthread_mutex_unlock(&m->mutex);
return st;
}
static double gettstop(){
return Times[STAGE_STOPPED];
static double gettstop(movemodel_t *m){
pthread_mutex_lock(&m->mutex);
double r = m->Times[STAGE_STOPPED];
pthread_mutex_unlock(&m->mutex);
return r;
}
movemodel_t trapez = {
.init_limits = initlims,
.stop = stop,
.emergency_stop = emstop,
.get_state = getst,
.calculate = calc,
.proc_move = proc,
.stoppenanotime = gettstop,
.stoppedtime = gettstop,
};

136
LibSidServo/serial.c
View File

@ -33,13 +33,14 @@
#include "main.h"
#include "movingmodel.h"
#include "serial.h"
#include "ssii.h"
// serial devices FD
static int encfd[2] = {-1, -1}, mntfd = -1;
// main mount data
static mountdata_t mountdata = {0};
// last encoders time and last encoders data - for speed measurement
static coordval_t lastXenc = {0}, lastYenc = {0};
//static coordval_t lastXenc = {0}, lastYenc = {0};
// mutexes for RW operations with mount device and data
static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
@ -57,17 +58,42 @@ typedef struct __attribute__((packed)){
} enc_t;
// calculate current X/Y speeds
static void getXspeed(double t){
void getXspeed(){
static less_square_t *ls = NULL;
if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
double speed = LS_calc_slope(ls, mountdata.encXposition.val, mountdata.encXposition.t);
if(fabs(speed) < 1.5 * MCC_MAX_X_SPEED){
mountdata.encXspeed.val = speed;
mountdata.encXspeed.t = mountdata.encXposition.t;
}
//DBG("Xspeed=%g", mountdata.encXspeed.val);
#if 0
mountdata.encXspeed.val = (mountdata.encXposition.val - lastXenc.val) / (t - lastXenc.t);
mountdata.encXspeed.t = (lastXenc.t + t) / 2.;
mountdata.encXspeed.t = (lastXenc.t + mountdata.encXposition.t) / 2.;
lastXenc.val = mountdata.encXposition.val;
lastXenc.t = t;
#endif
}
static void getYspeed(double t){
void getYspeed(){
static less_square_t *ls = NULL;
if(!ls){
ls = LS_init(Conf.EncoderSpeedInterval / Conf.EncoderReqInterval);
if(!ls) return;
}
double speed = LS_calc_slope(ls, mountdata.encYposition.val, mountdata.encYposition.t);
if(fabs(speed) < 1.5 * MCC_MAX_Y_SPEED){
mountdata.encYspeed.val = speed;
mountdata.encYspeed.t = mountdata.encYposition.t;
}
#if 0
mountdata.encYspeed.val = (mountdata.encYposition.val - lastYenc.val) / (t - lastYenc.t);
mountdata.encYspeed.t = (lastYenc.t + t) / 2.;
mountdata.encYspeed.t = (lastYenc.t + mountdata.encYposition.t) / 2.;
lastYenc.val = mountdata.encYposition.val;
lastYenc.t = t;
#endif
}
/**
@ -115,8 +141,9 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], double t){
DBG("Got positions X/Y= %.6g / %.6g", mountdata.encXposition.val, mountdata.encYposition.val);
mountdata.encXposition.t = t;
mountdata.encYposition.t = t;
if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
getXspeed(); getYspeed();
pthread_mutex_unlock(&datamutex);
//DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
}
@ -222,6 +249,31 @@ static int getmntbyte(){
}while(1);
return (int)byte;
}
// clear data from input buffer
static void clrmntbuf(){
if(mntfd < 0) return;
uint8_t byte;
fd_set rfds;
//double t0 = nanotime();
//int n = 0;
do{
FD_ZERO(&rfds);
FD_SET(mntfd, &rfds);
struct timeval tv = {.tv_sec=0, .tv_usec=10};
int retval = select(mntfd + 1, &rfds, NULL, NULL, &tv);
if(retval < 0){
if(errno == EINTR) continue;
DBG("Error in select()");
break;
}
if(FD_ISSET(mntfd, &rfds)){
ssize_t l = read(mntfd, &byte, 1);
if(l != 1) break;
//++n;
} else break;
}while(1);
//DBG("Cleared by %g (got %d bytes)", nanotime() - t0, n);
}
// main encoder thread (for separate encoder): read next data and make parsing
static void *encoderthread1(void _U_ *u){
@ -273,12 +325,14 @@ static void *encoderthread2(void _U_ *u){
mountdata.encXposition.val = X_ENC2RAD(v);
//DBG("encX(%g) = %g", t, mountdata.encXposition.val);
mountdata.encXposition.t = t;
if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed(t);
//if(t - lastXenc.t > Conf.EncoderSpeedInterval) getXspeed();
getXspeed();
if(getencval(encfd[1], &v, &t)){
mountdata.encYposition.val = Y_ENC2RAD(v);
//DBG("encY(%g) = %g", t, mountdata.encYposition.val);
mountdata.encYposition.t = t;
if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed(t);
//if(t - lastYenc.t > Conf.EncoderSpeedInterval) getYspeed();
getYspeed();
errctr = 0;
need2ask = 0;
} else {
@ -326,13 +380,14 @@ static void *mountthread(void _U_ *u){
int errctr = 0;
uint8_t buf[2*sizeof(SSstat)];
SSstat *status = (SSstat*) buf;
bzero(&mountdata, sizeof(mountdata));
if(Conf.RunModel) while(1){
pthread_mutex_lock(&datamutex);
// now change data
getModData(&mountdata);
pthread_mutex_unlock(&datamutex);
double t0 = nanotime();
while(nanotime() - t0 < Conf.MountReqInterval) usleep(500);
while(nanotime() - t0 < Conf.EncoderReqInterval) usleep(50);
t0 = nanotime();
}
// data to get
@ -342,8 +397,32 @@ static void *mountthread(void _U_ *u){
if(!cmd_getstat) goto failed;
while(mntfd > -1 && errctr < MAX_ERR_CTR){
// read data to status
double tgot = nanotime(), t0 = tgot;
//DBG("tgot=%g", tgot);
double t0 = nanotime();
#if 0
// 127 milliseconds to get answer on X/Y commands!!!
int64_t ans;
int ctr = 0;
if(SSgetint(CMD_MOTX, &ans)){
pthread_mutex_lock(&datamutex);
mountdata.motXposition.t = tgot;
mountdata.motXposition.val = X_MOT2RAD(ans);
pthread_mutex_unlock(&datamutex);
++ctr;
}
tgot = nanotime();
if(SSgetint(CMD_MOTY, &ans)){
pthread_mutex_lock(&datamutex);
mountdata.motXposition.t = tgot;
mountdata.motXposition.val = X_MOT2RAD(ans);
pthread_mutex_unlock(&datamutex);
++ctr;
}
if(ctr == 2){
mountdata.millis = (uint32_t)(1e3 * tgot);
DBG("Got both coords; millis=%d", mountdata.millis);
}
#endif
// 80 milliseconds to get answer on GETSTAT
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
#ifdef EBUG
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
@ -359,11 +438,12 @@ static void *mountthread(void _U_ *u){
errctr = 0;
pthread_mutex_lock(&datamutex);
// now change data
SSconvstat(status, &mountdata, tgot);
SSconvstat(status, &mountdata, t0);
pthread_mutex_unlock(&datamutex);
//DBG("GOT FULL stat by %g", nanotime() - t0);
// allow writing & getters
do{
usleep(5000);
usleep(500);
}while(nanotime() - t0 < Conf.MountReqInterval);
t0 = nanotime();
}
@ -439,7 +519,7 @@ int openEncoder(){
// return FALSE if failed
int openMount(){
if(Conf.RunModel) return TRUE;
if(Conf.RunModel) goto create_thread;
if(mntfd > -1) close(mntfd);
DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
@ -456,10 +536,13 @@ int openMount(){
}while(1);*/
mntRtmout.tv_sec = 0;
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes * 10bits / speed
create_thread:
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
DBG("Can't create thread");
close(mntfd);
mntfd = -1;
DBG("Can't create mount thread");
if(!Conf.RunModel){
close(mntfd);
mntfd = -1;
}
return FALSE;
}
DBG("Mount opened, thread started");
@ -512,6 +595,7 @@ void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
// write-read without locking mutex (to be used inside other functions)
static int wr(const data_t *out, data_t *in, int needeol){
if((!out && !in) || mntfd < 0) return FALSE;
clrmntbuf();
//double t0 = nanotime();
if(out){
if(out->len != (size_t)write(mntfd, out->buf, out->len)){
@ -525,9 +609,10 @@ static int wr(const data_t *out, data_t *in, int needeol){
}
}
//DBG("sent by %g", nanotime() - t0);
uint8_t buf[256];
data_t dumb = {.buf = buf, .maxlen = 256};
if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
//uint8_t buf[256];
//data_t dumb = {.buf = buf, .maxlen = 256};
if(!in) return TRUE;
//if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
in->len = 0;
for(size_t i = 0; i < in->maxlen; ++i){
int b = getmntbyte();
@ -585,26 +670,25 @@ static int bincmd(uint8_t *cmd, int len){
int ret = FALSE;
pthread_mutex_lock(&mntmutex);
// dummy buffer to clear trash in input
char ans[300];
data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
//char ans[300];
//data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
if(len == sizeof(SSscmd)){
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Short command");
#ifdef EBUG
logscmd((SSscmd*)cmd);
#endif
if(!wr(dscmd, &a, 1)) goto rtn;
if(!wr(dscmd, NULL, 1)) goto rtn;
}else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command");
#ifdef EBUG
loglcmd((SSlcmd*)cmd);
#endif
if(!wr(dlcmd, &a, 1)) goto rtn;
if(!wr(dlcmd, NULL, 1)) goto rtn;
}else{
goto rtn;
}
DBG("Write %d bytes and wait for ans", len);
data_t d;
d.buf = cmd;
d.len = d.maxlen = len;

2
LibSidServo/serial.h
View File

@ -40,3 +40,5 @@ int MountWriteReadRaw(const data_t *out, data_t *in);
int cmdS(SSscmd *cmd);
int cmdL(SSlcmd *cmd);
int cmdC(SSconfig *conf, int rw);
void getXspeed();
void getYspeed();

4
LibSidServo/ssii.c
View File

@ -78,6 +78,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, double t){
m->encXposition.val = X_ENC2RAD(s->Xenc);
m->encYposition.val = Y_ENC2RAD(s->Yenc);
m->encXposition.t = m->encYposition.t = t;
getXspeed(); getYspeed();
}
m->keypad = s->keypad;
m->extradata.ExtraBits = s->ExtraBits;
@ -164,9 +165,7 @@ int SSstop(int emerg){
const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
for(; i < 10; ++i){
DBG("t1: %g", nanotime());
if(!SStextcmd(cmdx, NULL)) continue;
DBG("t2: %g", nanotime());
if(SStextcmd(cmdy, NULL)) break;
}
if(i == 10) return FALSE;
@ -176,6 +175,7 @@ int SSstop(int emerg){
// update motors' positions due to encoders'
mcc_errcodes_t updateMotorPos(){
mountdata_t md = {0};
if(Conf.RunModel) return MCC_E_OK;
double t0 = nanotime(), t = 0.;
DBG("start @ %g", t0);
do{