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

Move the library to the separate repository

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2026-03-12 08:49:36 +03:00
commit 4dcb7d4444
98 changed files with 9089 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
examples/CMakeLists.txt Normal file
View File

@@ -0,0 +1,14 @@
project(examples)
# common includes & library
include_directories(../)
link_libraries(sidservo usefull_macros -lm)
# exe list
add_executable(goto goto.c dump.c conf.c)
add_executable(dump dumpmoving.c dump.c conf.c)
add_executable(dump_s dumpmoving_scmd.c dump.c conf.c)
add_executable(dumpswing dumpswing.c dump.c conf.c)
add_executable(traectory_s scmd_traectory.c dump.c traectories.c conf.c)
add_executable(SSIIconf SSIIconf.c conf.c)
add_executable(slewNtrack dumpmoving_dragNtrack.c dump.c conf.c)

28
examples/Readme.md Normal file
View File

@@ -0,0 +1,28 @@
Some examples of usage of libsidservo
=====================================
## Auxiliary files
*conf.c*, *conf.h* - base configuration - read from file (default: servo.conf) - to simplify examples running when config changes
*dump.c*, *dump.h* - base logging and dumping functions, also some useful functions like get current position and move to zero if current position isn't at zero.
*traectories.c*, *traectories.h* - modeling simple moving object traectories; also some functions like get current position in encoders' angles setting to zero at motors' zero.
*simpleconv.h*
## Examples
*dumpmoving.c* (`dump`) - dump moving relative starting point by simplest text commands "X" and "Y".
*dumpmoving_scmd.c* (`dump_s`) - moving relative starting point using "short" binary command.
*dumpswing.c* (`dumpswing`) - shake telescope around starting point by one of axis.
*goto.c* (`goto`) - get current coordinates or go to given (by simplest "X/Y" commands).
*scmd_traectory.c* (`traectory_s`) - try to move around given traectory using "short" binary commands.
*SSIIconf.c* (`SSIIconf`) - read/write hardware configuration of controller

163
examples/SSIIconf.c Normal file
View File

@@ -0,0 +1,163 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <math.h>
#include <stdio.h>
#include <usefull_macros.h>
#include "conf.h"
#include "sidservo.h"
#include "simpleconv.h"
typedef struct{
int help;
int helpargs;
int writeconf;
char *conffile;
char *hwconffile;
} parameters;
static hardware_configuration_t HW = {0};
static parameters G = {
.conffile = "servo.conf",
};
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"help-opts", NO_ARGS, NULL, 'H', arg_int, APTR(&G.helpargs), "configuration help"},
{"serconf", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "serial configuration file name"},
{"hwconf", NEED_ARG, NULL, 'i', arg_string, APTR(&G.hwconffile),"SSII configuration file name"},
{"writeconf", NO_ARGS, NULL, 0, arg_int, APTR(&G.writeconf), "write configuration (BE CAREFUL!)"},
end_option
};
static sl_option_t confopts[] = {
{"Xaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Xconf.accel), "X Default Acceleration, rad/s^2"},
{"Yaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Yconf.accel), "Y Default Acceleration, rad/s^2"},
end_option
};
static void dumpaxis(char axis, axis_config_t *c){
#define STRUCTPAR(p) (c)->p
#define DUMP(par) do{printf("%c%s=%.10g\n", axis, #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%c%s=%g\n", axis, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
DUMPD(accel);
DUMPD(backlash);
DUMPD(errlimit);
DUMP(propgain);
DUMP(intgain);
DUMP(derivgain);
DUMP(outplimit);
DUMP(currlimit);
DUMP(intlimit);
DUMP(motor_stepsperrev);
DUMP(axis_stepsperrev);
#undef DUMP
#undef DUMPD
}
static void dumpxbits(xbits_t *c){
#define DUMPBIT(f) do{printf("X%s=%d\n", #f, STRUCTPAR(f));}while(0)
DUMPBIT(motrev);
DUMPBIT(motpolarity);
DUMPBIT(encrev);
DUMPBIT(dragtrack);
DUMPBIT(trackplat);
DUMPBIT(handpaden);
DUMPBIT(newpad);
DUMPBIT(guidemode);
#undef DUMPBIT
}
static void dumpybits(ybits_t *c){
#define DUMPBIT(f) do{printf("Y%s=%d\n", #f, STRUCTPAR(f));}while(0)
DUMPBIT(motrev);
DUMPBIT(motpolarity);
DUMPBIT(encrev);
DUMPBIT(slewtrack);
DUMPBIT(digin_sens);
printf("Ydigin=%d\n", c->digin);
#undef DUMPBIT
}
static void dumpHWconf(){
#undef STRUCTPAR
#define STRUCTPAR(p) (HW).p
#define DUMP(par) do{printf("%s=%g\n", #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
green("X axis configuration:\n");
dumpaxis('X', &HW.Xconf);
green("X bits:\n");
dumpxbits(&HW.xbits);
green("Y axis configuration:\n");
dumpaxis('Y', &HW.Yconf);
green("Y bits:\n");
dumpybits(&HW.ybits);
green("Other:\n");
printf("address=%d\n", HW.address);
DUMP(eqrate);
DUMP(eqadj);
DUMP(trackgoal);
DUMPD(latitude);
DUMPU32(Xsetpr);
DUMPU32(Ysetpr);
DUMPU32(Xmetpr);
DUMPU32(Ymetpr);
DUMPD(Xslewrate);
DUMPD(Yslewrate);
DUMPD(Xpanrate);
DUMPD(Ypanrate);
DUMPD(Xguiderate);
DUMPD(Yguiderate);
DUMPU32(baudrate);
DUMPD(locsdeg);
DUMPD(locsspeed);
DUMPD(backlspd);
}
int main(int argc, char** argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help)
sl_showhelp(-1, cmdlnopts);
if(G.helpargs)
sl_showhelp(-1, confopts);
conf_t *sconf = readServoConf(G.conffile);
if(!sconf){
dumpConf();
return 1;
}
if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
/*
char *c = sl_print_opts(confopts, TRUE);
green("Got configuration:\n");
printf("%s\n", c);
FREE(c);
*/
dumpHWconf();
/*
if(G.hwconffile && G.writeconf){
;
}*/
Mount.quit();
return 0;
}

128
examples/conf.c Normal file
View File

@@ -0,0 +1,128 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <usefull_macros.h>
#include "conf.h"
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
.EncoderXDevPath = "/dev/encoder_X0",
.EncoderYDevPath = "/dev/encoder_Y0",
.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.EncoderReqInterval = 0.001,
.SepEncoder = 2,
.EncoderSpeedInterval = 0.05,
.EncodersDisagreement = 1e-5, // 2''
.PIDMaxDt = 1.,
.PIDRefreshDt = 0.1,
.PIDCycleDt = 5.,
.XPIDC.P = 0.5,
.XPIDC.I = 0.1,
.XPIDC.D = 0.2,
.XPIDV.P = 0.09,
.XPIDV.I = 0.0,
.XPIDV.D = 0.05,
.YPIDC.P = 0.5,
.YPIDC.I = 0.1,
.YPIDC.D = 0.2,
.YPIDV.P = 0.09,
.YPIDV.I = 0.0,
.YPIDV.D = 0.05,
.MaxPointingErr = 0.13962634,
.MaxFinePointingErr = 0.026179939,
.MaxGuidingErr = 4.8481368e-7,
};
static sl_option_t opts[] = {
{"MountDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.MountDevPath), "path to mount device"},
{"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of mount device"},
{"EncoderDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.EncoderDevPath), "path to encoder device"},
{"EncoderDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.EncoderDevSpeed), "serial speed of encoder device"},
{"SepEncoder", NEED_ARG, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device (1 - one device, 2 - two devices)"},
{"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)"},
{"EncodersDisagreement", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncodersDisagreement),"acceptable disagreement between motor and axis encoders"},
{"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
{"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
{"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"},
{"PIDMaxDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDMaxDt), "maximal PID refresh time interval (if larger all old data will be cleared)"},
{"PIDRefreshDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDRefreshDt), "normal PID refresh interval by master process"},
{"PIDCycleDt", NEED_ARG, NULL, 0, arg_double, APTR(&Config.PIDCycleDt), "PID I cycle time (analog of \"RC\" for PID on opamps)"},
{"XPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.P), "P of X PID (coordinate driven)"},
{"XPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.I), "I of X PID (coordinate driven)"},
{"XPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDC.D), "D of X PID (coordinate driven)"},
{"YPIDCP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.P), "P of Y PID (coordinate driven)"},
{"YPIDCI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.I), "I of Y PID (coordinate driven)"},
{"YPIDCD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDC.D), "D of Y PID (coordinate driven)"},
{"XPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.P), "P of X PID (velocity driven)"},
{"XPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.I), "I of X PID (velocity driven)"},
{"XPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.XPIDV.D), "D of X PID (velocity driven)"},
{"YPIDVP", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.P), "P of Y PID (velocity driven)"},
{"YPIDVI", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.I), "I of Y PID (velocity driven)"},
{"YPIDVD", NEED_ARG, NULL, 0, arg_double, APTR(&Config.YPIDV.D), "D of Y PID (velocity driven)"},
{"MaxPointingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxPointingErr), "if angle < this, change state from \"slewing\" to \"pointing\" (coarse pointing): 8 degrees"},
{"MaxFinePointingErr",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxFinePointingErr), "if angle < this, chane state from \"pointing\" to \"guiding\" (fine poinging): 1.5 deg"},
{"MaxGuidingErr", NEED_ARG, NULL, 0, arg_double, APTR(&Config.MaxGuidingErr), "if error less than this value we suppose that target is captured and guiding is good (true guiding): 0.1''"},
{"XEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.XEncZero), "X axis encoder approximate zero position"},
{"YEncZero", NEED_ARG, NULL, 0, arg_int, APTR(&Config.YEncZero), "Y axis encoder approximate zero position"},
// {"",NEED_ARG, NULL, 0, arg_double, APTR(&Config.), ""},
end_option
};
conf_t *readServoConf(const char *filename){
if(!filename) filename = DEFCONFFILE;
int n = sl_conf_readopts(filename, opts);
if(n < 0){
WARNX("Can't read file %s", filename);
return NULL;
}
if(n == 0){
WARNX("Got ZERO parameters from %s", filename);
return NULL;
}
return &Config;
}
void dumpConf(){
char *c = sl_print_opts(opts, TRUE);
printf("Current configuration:\n%s\n", c);
FREE(c);
}
void confHelp(){
sl_conf_showhelp(-1, opts);
}
const char* errcodes[MCC_E_AMOUNT] = {
[MCC_E_OK] = "OK",
[MCC_E_FATAL] = "Fatal error",
[MCC_E_BADFORMAT] = "Wrong data format",
[MCC_E_ENCODERDEV] = "Encoder error",
[MCC_E_MOUNTDEV] = "Mount error",
[MCC_E_FAILED] = "Failed to run"
};
// return string with error code
const char *EcodeStr(mcc_errcodes_t e){
if(e >= MCC_E_AMOUNT) return "Wrong error code";
return errcodes[e];
}

28
examples/conf.h Normal file
View File

@@ -0,0 +1,28 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "sidservo.h"
#define DEFCONFFILE "servo.conf"
void confHelp();
conf_t *readServoConf(const char *filename);
void dumpConf();
const char *EcodeStr(mcc_errcodes_t e);

197
examples/dump.c Normal file
View File

@@ -0,0 +1,197 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// logging of mount position
#include <usefull_macros.h>
#include "dump.h"
#include "simpleconv.h"
// starting dump time (to conform different logs)
static struct timespec dumpT0 = {0};
#if 0
// amount of elements used for encoders' data filtering
#define NFILT (10)
static double filterK[NFILT];
static double lastvals[2][NFILT] = {0};
static int need2buildFilter = 1;
static void buildFilter(){
filterK[NFILT-1] = 1.;
double sum = 1.;
for(int i = NFILT-2; i > -1; --i){
filterK[i] = (filterK[i+1] + 1.) * 1.1;
sum += filterK[i];
}
for(int i = 0; i < NFILT; ++i) filterK[i] /= sum;
}
static double filter(double val, int idx){
if(need2buildFilter){
buildFilter();
need2buildFilter = 0;
}
static int ctr[2] = {0};
for(int i = NFILT-1; i > 0; --i) lastvals[idx][i] = lastvals[idx][i-1];
lastvals[idx][0] = val;
double r = 0.;
if(ctr[idx] < NFILT){
++ctr[idx];
return val;
}
for(int i = 0; i < NFILT; ++i) r += filterK[i] * lastvals[idx][i];
return r;
}
#endif
// return starting time of dump
void dumpt0(struct timespec *t){
if(t) *t = dumpT0;
}
/**
* @brief logmnt - log mount data into file
* @param fcoords - file to dump
* @param m - mount data
*/
void logmnt(FILE *fcoords, mountdata_t *m){
if(!fcoords) return;
//DBG("LOG %s", m ? "data" : "header");
if(!m){ // write header
fprintf(fcoords, " time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
return;
}else if(dumpT0.tv_sec == 0) dumpT0 = m->encXposition.t;
// write data
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
Mount.timeDiff(&m->encXposition.t, &dumpT0), RAD2DEG(m->motXposition.val), RAD2DEG(m->motYposition.val),
RAD2DEG(m->encXposition.val), RAD2DEG(m->encYposition.val),
RAD2DEG(m->encXspeed.val), RAD2DEG(m->encYspeed.val),
m->millis);
fflush(fcoords);
}
/**
* @brief dumpmoving - dump conf while moving
* @param fcoords - dump file
* @param t - max waiting time
* @param N - number of cycles to wait while motors aren't moving
*/
void dumpmoving(FILE *fcoords, double t, int N){
if(!fcoords) return;
mountdata_t mdata;
DBG("Start dump");
int ntries = 0;
for(; ntries < 10; ++ntries){
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
}
if(ntries == 10){
WARNX("Can't get mount data");
LOGWARN("Can't get mount data");
}
uint32_t mdmillis = mdata.millis;
struct timespec encXt = mdata.encXposition.t;
int ctr = -1;
double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val;
double t0 = Mount.timeFromStart();
while(Mount.timeFromStart() - t0 < t && ctr < N){
usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
//double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
struct timespec msrt = mdata.encXposition.t;
if(msrt.tv_nsec == encXt.tv_nsec) continue;
encXt = msrt;
if(fcoords) logmnt(fcoords, &mdata);
if(mdata.millis == mdmillis) continue;
//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;
ylast = mdata.motYposition.val;
ctr = 0;
}else ++ctr;
}
DBG("Exit dumping; tend=%g, tmon=%g", t, Mount.timeFromStart() - t0);
}
/**
* @brief waitmoving - wait until moving by both axiss stops at least for N cycles
* @param N - amount of stopped cycles
*/
void waitmoving(int N){
mountdata_t mdata;
int ctr = -1;
uint32_t millis = 0;
//double xlast = 0., ylast = 0.;
DBG("Wait moving for %d stopped times", N);
while(ctr < N){
usleep(10000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(mdata.Xstate != AXIS_STOPPED || mdata.Ystate != AXIS_STOPPED) ctr = 0;
else ++ctr;
}
}
/**
* @brief getPos - get current position
* @param mot (o) - motor position (or NULL)
* @param Y (o) - encoder position (or NULL)
* @return FALSE if failed
*/
int getPos(coordval_pair_t *mot, coordval_pair_t *enc){
mountdata_t mdata = {0};
int errcnt = 0;
do{
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
else{
errcnt = 0;
if(mdata.millis) break;
}
}while(errcnt < 10);
if(errcnt >= 10){
WARNX("Can't read mount status");
return FALSE;
}
if(mot){
mot->X = mdata.motXposition;
mot->Y = mdata.motYposition;
}
if(enc){
enc->X = mdata.encXposition;
enc->Y = mdata.encYposition;
}
return TRUE;
}
// check current position and go to 0 if non-zero
void chk0(int ncycles){
coordval_pair_t M;
if(!getPos(&M, NULL)) signals(2);
if(M.X.val || M.Y.val){
WARNX("Mount position isn't @ zero; moving");
coordpair_t zero = {0., 0.};
Mount.moveTo(&zero);
waitmoving(ncycles);
green("Now mount @ zero\n");
}
}

30
examples/dump.h Normal file
View File

@@ -0,0 +1,30 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdio.h>
#include "sidservo.h"
void logmnt(FILE *fcoords, mountdata_t *m);
void dumpmoving(FILE *fcoords, double t, int N);
void waitmoving(int N);
int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
void chk0(int ncycles);
void dumpt0(struct timespec *t);

105
examples/dumpmoving.c Normal file
View File

@@ -0,0 +1,105 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// dump telescope moving using simplest goto command
#include <math.h>
#include <signal.h>
#include <stdio.h>
#include <time.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
typedef struct{
int help;
int verbose;
int Ncycles;
char *logfile;
char *coordsoutput;
char *conffile;
} parameters;
static parameters G = {
.Ncycles = 40,
};
static FILE *fcoords = NULL;
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"verbose", NO_ARGS, NULL, 'v', arg_none, APTR(&G.verbose), "verbose level (each -v adds 1)"},
{"logfile", NEED_ARG, NULL, 'l', arg_string, APTR(&G.logfile), "log file name"},
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"},
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
end_option
};
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
LOGERR("Exit with status %d", sig);
Mount.quit();
exit(sig);
}
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR;
if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1;
if(G.logfile) OPENLOG(G.logfile, lvl, 1);
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();
confHelp();
return 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
logmnt(fcoords, NULL);
time_t curtime = time(NULL);
LOGMSG("Started @ %s", ctime(&curtime));
LOGMSG("Mount device %s @ %d", Config->MountDevPath, Config->MountDevSpeed);
LOGMSG("Encoder device %s @ %d", Config->EncoderDevPath, Config->EncoderDevSpeed);
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init devices");
coordval_pair_t M;
if(!getPos(&M, NULL)) ERRX("Can't get current position");
signal(SIGTERM, signals); // kill (-15) - quit
signal(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit
signal(SIGQUIT, signals); // ctrl+\ - quit
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
coordpair_t tag = {.X = DEG2RAD(45.) + M.X.val, .Y = DEG2RAD(45.) + M.Y.val};
if(MCC_E_OK != Mount.moveTo(&tag))
ERRX("Can't move to 45, 45");
dumpmoving(fcoords, 30., G.Ncycles);
tag.X = M.X.val; tag.Y = M.Y.val;
Mount.moveTo(&tag);
dumpmoving(fcoords, 30., G.Ncycles);
signals(0);
return 0;
}

228
examples/dumpmoving_dragNtrack.c Normal file
View File

@@ -0,0 +1,228 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// move telescope to target using short command and force it to track mode
// also do some corrections while moving
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
// Original XXI=6827
// XXD=136546
// XXB=4915666
typedef struct{
int help;
int Ncycles;
double reqint;
char *coordsoutput;
char *conffile;
} parameters;
static parameters G = {
.Ncycles = 40,
.reqint = -1.,
};
static FILE *fcoords = NULL;
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"},
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1)"},
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
end_option
};
static mcc_errcodes_t return2zero();
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
return2zero();
sleep(5);
Mount.quit();
exit(sig);
}
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
// return TRUE if motor position is reached +- 0.01 degrees
#define XYcount (DEG2RAD(0.3))
// tag in degrees!
static int Wait(double tag, int isX){
mountdata_t mdata;
red("Wait for %g degrees\n", tag);
tag = DEG2RAD(tag);
int errcnt = 0;
uint32_t millis = 0;
double curpos = 0.;
double t0 = sl_dtime();
do{
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
else{
errcnt = 0;
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(isX) curpos = mdata.motXposition.val;
else curpos = mdata.motYposition.val;
}
double t = sl_dtime();
if(t - t0 > 1.){
t0 = t;
printf("\t\tCurrent MOT X/Y: %g / %g deg\n", RAD2DEG(mdata.motXposition.val),
RAD2DEG(mdata.motYposition.val));
}
}while(fabs(curpos - tag) > XYcount && errcnt < 10);
if(errcnt >= 10){
WARNX("Too much errors");
return FALSE;
}
green("%s reached position %g degrees\n", (isX) ? "X" : "Y", RAD2DEG(tag));
fflush(stdout);
return TRUE;
}
// previous GOTO coords/speeds for `mkcorr`
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(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);
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;
cmd.XBits = 108;
cmd.YBits = 28;*/
return Mount.shortCmd(&cmd);
}
static mcc_errcodes_t return2zero(){
short_command_t cmd = {0};
DBG("Try to move to zero");
cmd.Xmot = 0.; cmd.Ymot = 0.;
coordpair_t maxspd;
if(MCC_E_OK != Mount.getMaxSpeed(&maxspd)) return MCC_E_FAILED;
cmd.Xspeed = maxspd.X;
cmd.Yspeed = maxspd.Y;
/*cmd.xychange = 1;
cmd.XBits = 100;
cmd.YBits = 20;*/
return Mount.shortCmd(&cmd);
}
static mcc_errcodes_t mkcorr(coordpair_t *adder, coordpair_t *time){
long_command_t cmd = {0};
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);
}
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();
return 1;
}
if(G.reqint > 0.) Config->MountReqInterval = G.reqint;
if(MCC_E_OK != Mount.init(Config)){
WARNX("Can't init devices");
return 1;
}
//if(!getPos(&M, NULL)) ERRX("Can't get current position");
signal(SIGTERM, signals); // kill (-15) - quit
signal(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit
signal(SIGQUIT, signals); // ctrl+\ - quit
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
// move to X=40 degr with different speeds
pthread_t dthr;
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
// move to 10/10
coordpair_t coords, speeds, adders, tadd;
coords = (coordpair_t){.X = 10., .Y = 20.};
speeds = (coordpair_t){.X = 1., .Y = 2.};
adders = (coordpair_t){.X = 0.01, .Y = 0.01};
tadd = (coordpair_t){.X = 1., .Y = 2.};
green("Goto\n");
if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go");
DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y);
green("Waiting X==4\n");
Wait(4., 1);
// now we are at point by Y but still moving by X; make small correction by X/Y into '+'
green("Mkcorr 1\n");
if(MCC_E_OK != mkcorr(&adders, &tadd)) ERRX("Can't make corr");
green("Waiting X==6\n");
Wait(6., 1);
green("Goto more\n");
coords = (coordpair_t){.X = 20., .Y = 30.};
if(MCC_E_OK != gotos(&coords, &speeds)) ERRX("Can't go");
DBG("c/s: %g %g %g %g", coords.X, coords.Y, speeds.X, speeds.Y);
green("Waiting Y==14\n");
Wait(14., 0);
// now we are @ point, make the same small correction again
green("Mkcorr 2\n");
if(MCC_E_OK != mkcorr(&coords, &speeds)) ERRX("Can't make corr");
// wait for 5 seconds
green("Wait for 5 seconds\n");
sleep(5);
// return to zero and wait
green("Return 2 zero and wait\n");
if(MCC_E_OK != return2zero()) ERRX("Can't return");
Wait(0., 0);
Wait(0., 1);
// wait moving ends
pthread_join(dthr, NULL);
signals(0);
return 0;
}

177
examples/dumpmoving_scmd.c Normal file
View File

@@ -0,0 +1,177 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// dump telescope moving using short binary commands
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
typedef struct{
int help;
int Ncycles;
int relative;
double reqint;
char *coordsoutput;
char *conffile;
char *axis;
} parameters;
static parameters G = {
.Ncycles = 40,
.reqint = -1.,
.axis = "X",
};
static FILE *fcoords = NULL;
static coordval_pair_t M;
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"},
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1)"},
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X, Y or B for both)"},
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
{"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"},
end_option
};
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.quit();
exit(sig);
}
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
// return TRUE if motor position is reached +- 0.01 degrees
#define XYcount (DEG2RAD(0.01))
static int Wait(double tag){
mountdata_t mdata;
red("Wait for %g degrees\n", RAD2DEG(tag));
int errcnt = 0;
double sign = 0.;
uint32_t millis = 0;
double curpos = 0.;
do{
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
else{
errcnt = 0;
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(*G.axis == 'X') curpos = mdata.motXposition.val;
else curpos = mdata.motYposition.val;
if(sign == 0.) sign = (curpos > tag) ? 1. : -1.;
//printf("%s=%g deg, need %g deg; delta=%g arcmin\n", G.axis, RAD2DEG(curpos),
// RAD2DEG(tag), RAD2DEG(sign*(curpos - tag))*60.);
}
}while(sign*(curpos - tag) > XYcount && errcnt < 10);
if(errcnt >= 10){
WARNX("Too much errors");
return FALSE;
}
green("%s reached position %g degrees\n", G.axis, RAD2DEG(tag));
fflush(stdout);
return TRUE;
}
// move X/Y to 40 degr with given speed until given coord
static void move(double target, double limit, double speed){
green("Move %s to %g until %g with %gdeg/s\n", G.axis, target, limit, speed);
short_command_t cmd = {0};
if(*G.axis == 'X' || *G.axis == 'B'){
cmd.Xmot = DEG2RAD(target) + M.X.val;
cmd.Xspeed = DEG2RAD(speed);
limit = DEG2RAD(limit) + M.X.val;
}
if(*G.axis == 'Y' || *G.axis == 'B'){
cmd.Ymot = DEG2RAD(target) + M.Y.val;
cmd.Yspeed = DEG2RAD(speed);
if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y.val;
}
if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command");
if(!Wait(limit)) signals(9);
}
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
if(strcmp(G.axis, "X") && strcmp(G.axis, "Y") && strcmp(G.axis, "B")){
WARNX("\"Axis\" should be X, Y or B");
return 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();
return 1;
}
if(G.reqint > 0.) Config->MountReqInterval = G.reqint;
if(MCC_E_OK != Mount.init(Config)){
WARNX("Can't init devices");
return 1;
}
if(!getPos(&M, NULL)) ERRX("Can't get current position");
signal(SIGTERM, signals); // kill (-15) - quit
signal(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit
signal(SIGQUIT, signals); // ctrl+\ - quit
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
// move to X=40 degr with different speeds
pthread_t dthr;
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
// goto 30' with 5'/s
move(10., 30./60., 5./60.);
// goto 1' with 10'/s
move(10., 1., 10./60.);
// goto 3degr with 15'/s
move(10., 3., 15./60.);
// and go back with 7deg/s
move(0., 0., 7.);
// be sure to move @ starting position
coordpair_t tag = {.X = M.X.val, .Y = M.Y.val};
Mount.moveTo(&tag);
// wait moving ends
pthread_join(dthr, NULL);
signals(0);
return 0;
}

190
examples/dumpswing.c Normal file
View File

@@ -0,0 +1,190 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
// swing telescope by given axis with given period and max amplitude, reqinterval=0.05 (min)
typedef struct{
int help;
int Ncycles;
int Nswings;
double period;
double amplitude;
char *coordsoutput;
char *conffile;
char *axis;
} parameters;
static parameters G = {
.Ncycles = 20,
.axis = "X",
.Nswings = 10,
.period = 1.,
.amplitude = 5.,
};
static FILE *fcoords = NULL;
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 20)"},
{"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 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
};
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.quit();
exit(sig);
}
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
// wait until mount is stopped within 5 cycles or until time reached t
void waithalf(double t){
mountdata_t mdata;
int ctr = -1;
uint32_t millis = 0;
double xlast = 0., ylast = 0.;
while(ctr < 5){
if(Mount.timeFromStart() >= t) return;
usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
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));
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
ctr = 0;
}else{
//DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
++ctr;
}
}
}
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
if(strcmp(G.axis, "X") && strcmp(G.axis, "Y")){
WARNX("\"Axis\" should be X or Y");
return 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w"))){
WARNX("Can't open %s", G.coordsoutput);
return 1;
}
}else fcoords = stdout;
if(G.Ncycles < 2){
WARNX("Ncycles should be >2");
return 1;
}
double absamp = fabs(G.amplitude);
if(absamp < 0.01 || absamp > 45.){
WARNX("Amplitude should be from 0.01 to 45 degrees");
return 1;
}
if(G.period < 0.1 || G.period > 900.){
WARNX("Period should be from 0.1 to 900s");
return 1;
}
if(G.Nswings < 1){
WARNX("Nswings should be more than 0");
return 1;
}
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();
return 1;
}
mcc_errcodes_t e = Mount.init(Config);
if(e != MCC_E_OK){
WARNX("Can't init devices");
return 1;
}
signal(SIGTERM, signals); // kill (-15) - quit
signal(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit
signal(SIGQUIT, signals); // ctrl+\ - quit
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
pthread_t dthr;
chk0(G.Ncycles);
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
G.period /= 2.; // pause between commands
double tagX, tagY;
if(*G.axis == 'X'){
tagX = DEG2RAD(G.amplitude); tagY = 0.;
}else{
tagX = 0.; tagY = DEG2RAD(G.amplitude);
}
double t = Mount.timeFromStart(), t0 = t;
coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY};
double divide = 2.;
for(int i = 0; i < G.Nswings; ++i){
Mount.moveTo(&tag);
DBG("CMD: %g", Mount.timeFromStart()-t0);
t += G.period / divide;
divide = 1.;
waithalf(t);
DBG("Moved to +, t=%g", t-t0);
DBG("CMD: %g", Mount.timeFromStart()-t0);
Mount.moveTo(&rtag);
t += G.period;
waithalf(t);
DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", Mount.timeFromStart()-t0);
}
green("Move to zero @ %g\n", Mount.timeFromStart());
tag = (coordpair_t){0};
// be sure to move @ 0,0
if(MCC_E_OK != Mount.moveTo(&tag)){
Mount.emergStop();
Mount.moveTo(&tag);
}
// wait moving ends
pthread_join(dthr, NULL);
#undef SCMD
signals(0);
return 0;
}

145
examples/goto.c Normal file
View File

@@ -0,0 +1,145 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// move telescope to given MOTOR position in degrees
#include <signal.h>
#include <stdio.h>
#include <time.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
typedef struct{
int help;
int Ncycles;
int wait;
int relative;
char *coordsoutput;
char *conffile;
double X;
double Y;
} parameters;
static parameters G = {
.Ncycles = 10,
.X = NAN,
.Y = NAN,
};
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles of waiting in stopped state (default: 40)"},
{"newx", NEED_ARG, NULL, 'X', arg_double, APTR(&G.X), "new X coordinate"},
{"newy", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Y), "new Y coordinate"},
{"output", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"file to log coordinates"},
{"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), "wait until mowing stopped"},
{"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"},
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
end_option
};
static FILE* fcoords = NULL;
static pthread_t dthr;
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
DBG("Quit");
Mount.quit();
DBG("close");
if(fcoords) fclose(fcoords);
exit(sig);
}
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
int main(int _U_ argc, char _U_ **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help)
sl_showhelp(-1, cmdlnopts);
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();
return 1;
}
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
coordval_pair_t M, E;
if(!getPos(&M, &E)) ERRX("Can't get current position");
printf("Current time: %.10f\n", Mount.timeFromStart());
if(G.coordsoutput){
if(!G.wait) green("When logging I should wait until moving ends; added '-w'\n");
G.wait = 1;
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
}
M.X.val = RAD2DEG(M.X.val);
M.Y.val = RAD2DEG(M.Y.val);
printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", M.X.val, M.Y.val,
RAD2DEG(E.X.val), RAD2DEG(E.Y.val));
if(isnan(G.X) && isnan(G.Y)) goto out;
coordpair_t tag;
if(isnan(G.X)){
if(G.relative) G.X = 0.;
else G.X = M.X.val;
}
if(isnan(G.Y)){
if(G.relative) G.Y = 0.;
else G.Y = M.Y.val;
}
if(G.relative){
G.X += M.X.val;
G.Y += M.Y.val;
}
printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y);
tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y);
mcc_errcodes_t e = Mount.moveTo(&tag);
if(MCC_E_OK != e){
WARNX("Cant go to given coordinates: %s\n", EcodeStr(e));
goto out;
}
if(G.wait){
sleep(1);
waitmoving(G.Ncycles);
if(!getPos(&M, NULL)) WARNX("Can't get current position");
else printf("New mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
}
out:
DBG("JOIN");
if(G.coordsoutput) pthread_join(dthr, NULL);
DBG("QUIT");
if(G.wait){
usleep(250000); // pause to refresh coordinates
if(getPos(&M, &E)) printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val),
RAD2DEG(E.X.val), RAD2DEG(E.Y.val));
Mount.quit();
}
return 0;
}

188
examples/scmd_traectory.c Normal file
View File

@@ -0,0 +1,188 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <pthread.h>
#include <signal.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
#include "traectories.h"
// calculate some traectory and try to run over it
typedef struct{
int help;
int dumpconf;
int Ncycles; // n cycles to wait stop
double reqint; // requests interval (seconds)
double Xmax; // maximal X to stop
double Ymax; // maximal Y to stop
double tmax; // maximal time of emulation
double X0; // starting point of traectory (-30..30 degr)
double Y0; // -//-
char *coordsoutput; // dump file
char *errlog; // log with position errors
char *tfn; // traectory function name
char *conffile;
} parameters;
static conf_t *Config = NULL;
static FILE *fcoords = NULL, *errlog = NULL;
static pthread_t dthr;
static parameters G = {
.Ncycles = 40,
.reqint = 0.1,
.tfn = "sincos",
.Xmax = 45.,
.Ymax = 45.,
.tmax = 300., // 5 minutes
.X0 = 10.,
.Y0 = 10.,
};
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"ncycles", NEED_ARG, NULL, 'n', arg_int, APTR(&G.Ncycles), "N cycles in stopped state (default: 40)"},
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
{"reqinterval", NEED_ARG, NULL, 'i', arg_double, APTR(&G.reqint), "mount requests interval (default: 0.1 second)"},
{"traectory", NEED_ARG, NULL, 't', arg_string, APTR(&G.tfn), "used traectory function (default: sincos)"},
{"xmax", NEED_ARG, NULL, 'X', arg_double, APTR(&G.Xmax), "maximal abs X coordinate for traectory (default: 45 degrees)"},
{"ymax", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Ymax), "maximal abs Y coordinate for traectory (default: 45 degrees)"},
{"tmax", NEED_ARG, NULL, 'T', arg_double, APTR(&G.tmax), "maximal duration time of emulation (default: 300 seconds)"},
{"x0", NEED_ARG, NULL, '0', arg_double, APTR(&G.X0), "starting X-coordinate of traectory (default: 10 degrees)"},
{"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"},
{"conffile", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "configuration file name"},
{"errlog", NEED_ARG, NULL, 'e', arg_string, APTR(&G.errlog), "file with errors log"},
{"dumpconf", NO_ARGS, NULL, 'D', arg_int, APTR(&G.dumpconf), "dump current configuration"},
end_option
};
void signals(int sig){
if(sig){
signal(sig, SIG_IGN);
DBG("Get signal %d, quit.\n", sig);
}
Mount.stop();
sleep(1);
Mount.quit();
if(fcoords) fclose(fcoords);
exit(sig);
}
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
// calculate
static void runtraectory(traectory_fn tfn){
if(!tfn) return;
coordval_pair_t telXY;
coordval_pair_t target;
coordpair_t traectXY;
double tlast = 0., tstart = Mount.timeFromStart();
long tlastXnsec = 0, tlastYnsec = 0;
struct timespec tcur, t0 = {0};
dumpt0(&t0);
while(1){
if(!telpos(&telXY)){
WARNX("No next telescope position");
return;
}
if(!Mount.currentT(&tcur)) continue;
if(telXY.X.t.tv_nsec == tlastXnsec && telXY.Y.t.tv_nsec == tlastYnsec) continue; // last measure - don't mind
DBG("\n\nTELPOS: %g'/%g' (%.6f/%.6f)", RAD2AMIN(telXY.X.val), RAD2AMIN(telXY.Y.val), RAD2DEG(telXY.X.val), RAD2DEG(telXY.Y.val));
tlastXnsec = telXY.X.t.tv_nsec; tlastYnsec = telXY.Y.t.tv_nsec;
double t = Mount.timeFromStart();
if(fabs(telXY.X.val) > G.Xmax || fabs(telXY.Y.val) > G.Ymax || t - tstart > G.tmax) break;
if(!traectory_point(&traectXY, t)) break;
target.X.val = traectXY.X; target.Y.val = traectXY.Y;
target.X.t = target.Y.t = tcur;
if(t0.tv_nsec == 0 && t0.tv_sec == 0) dumpt0(&t0);
else{
//DBG("target: %g'/%g'", RAD2AMIN(traectXY.X), RAD2AMIN(traectXY.Y));
DBG("%g: dX=%.4f'', dY=%.4f''", t-tstart, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
//DBG("Correct to: %g/%g with EP %g/%g", RAD2DEG(target.X.val), RAD2DEG(target.Y.val), RAD2DEG(endpoint.X), RAD2DEG(endpoint.Y));
if(errlog)
fprintf(errlog, "%10.4f %10.4f %10.4f\n", Mount.timeDiff(&telXY.X.t, &t0), RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
}
if(MCC_E_OK != Mount.correctTo(&target)) WARNX("Error of correction!");
while((t = Mount.timeFromStart()) - tlast < Config->PIDRefreshDt) usleep(500);
tlast = t;
}
WARNX("No next traectory point or emulation ends");
}
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
if(G.Xmax < 1. || G.Xmax > 90.) ERRX("Xmax should be 1..90 degrees");
if(G.Ymax < 1. || G.Ymax > 90.) ERRX("Ymax should be 1..90 degrees");
// convert to radians
G.Xmax = DEG2RAD(G.Xmax); G.Ymax = DEG2RAD(G.Ymax);
if(G.X0 < -30. || G.X0 > 30. || G.Y0 < -30. || G.Y0 > 30.)
ERRX("X0 and Y0 should be -30..30 degrees");
if(G.errlog){
if(!(errlog = fopen(G.errlog, "w")))
ERRX("Can't open error log %s", G.errlog);
else
fprintf(errlog, "# time Xerr'' Yerr'' // target - real\n");
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
Config = readServoConf(G.conffile);
if(!Config || G.dumpconf){
dumpConf();
return 1;
}
Config->MountReqInterval = G.reqint;
traectory_fn tfn = traectory_by_name(G.tfn);
if(!tfn){
WARNX("Bad traectory name %s, should be one of", G.tfn);
print_tr_names();
return 1;
}
coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
if(!init_traectory(tfn, &c)){
ERRX("Can't init traectory");
return 1;
}
mcc_errcodes_t e = Mount.init(Config);
if(e != MCC_E_OK){
WARNX("Can't init devices");
return 1;
}
signal(SIGTERM, signals); // kill (-15) - quit
signal(SIGHUP, SIG_IGN); // hup - ignore
signal(SIGINT, signals); // ctrl+C - quit
signal(SIGQUIT, signals); // ctrl+\ - quit
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
chk0(G.Ncycles);
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
;
runtraectory(tfn);
signals(0);
return 0;
}

25
examples/servo.conf Normal file
View File

@@ -0,0 +1,25 @@
# Current configuration
MountDevPath=/dev/ttyUSB0
MountDevSpeed=19200
EncoderDevPath=(null)
EncoderDevSpeed=1000000
MountReqInterval=0.1
EncoderReqInterval=0.001
SepEncoder=2
EncoderXDevPath=/dev/encoder_X0
EncoderYDevPath=/dev/encoder_Y0
EncoderSpeedInterval=0.05
RunModel=1
XPIDCP=0.8
XPIDCI=0.0
XPIDCD=0.0
YPIDCP=0.5
YPIDCI=0.0
YPIDCD=0.0
XPIDVP=0.2
XPIDVI=0.1
XPIDVD=0.0
YPIDVP=0.2
YPIDVI=0.1
YPIDVD=0.0

29
examples/simpleconv.h Normal file
View File

@@ -0,0 +1,29 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// simple conversion macros
#include <math.h>
#define DEG2RAD(d) ((d)/180.*M_PI)
#define ASEC2RAD(d) ((d)/180.*M_PI/3600.)
#define AMIN2RAD(d) ((d)/180.*M_PI/60.)
#define RAD2DEG(r) ((r)/M_PI*180.)
#define RAD2ASEC(r) ((r)/M_PI*180.*3600.)
#define RAD2AMIN(r) ((r)/M_PI*180.*60.)

136
examples/traectories.c Normal file
View File

@@ -0,0 +1,136 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// some simplest traectories
// all traectories runs increasing X and Y from starting point
#include <stdio.h>
#include <string.h>
#include <usefull_macros.h>
#include "simpleconv.h"
#include "traectories.h"
static traectory_fn cur_traectory = NULL;
// starting point of traectory
static coordpair_t XYstart = {0};
static double tstart = 0.;
/**
* @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
* @param f - function calculating next point
* @param XY0 - starting point
* @return FALSE if failed
*/
int init_traectory(traectory_fn f, coordpair_t *XY0){
if(!f || !XY0) return FALSE;
cur_traectory = f;
XYstart = *XY0;
tstart = Mount.timeFromStart();
mountdata_t mdata;
int ntries = 0;
for(; ntries < 10; ++ntries){
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
}
if(ntries == 10) return FALSE;
return TRUE;
}
/**
* @brief traectory_point - get traectory point for given time
* @param nextpt (o) - next point coordinates
* @param t - UNIX-time of event
* @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180)
*/
int traectory_point(coordpair_t *nextpt, double t){
if(t < 0. || !cur_traectory) return FALSE;
coordpair_t pt;
if(!cur_traectory(&pt, t)) return FALSE;
if(nextpt) *nextpt = pt;
if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE;
return TRUE;
}
// current telescope position according to starting motor coordinates
// @return FALSE if failed to get current coordinates
int telpos(coordval_pair_t *curpos){
mountdata_t mdata;
int ntries = 0;
for(; ntries < 10; ++ntries){
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
}
if(ntries == 10) return FALSE;
coordval_pair_t pt;
//DBG("\n\nTELPOS: %g'/%g' measured @ %.6f", RAD2AMIN(mdata.encXposition.val), RAD2AMIN(mdata.encYposition.val), mdata.encXposition.t);
pt.X.val = mdata.encXposition.val;
pt.Y.val = mdata.encYposition.val;
pt.X.t = mdata.encXposition.t;
pt.Y.t = mdata.encYposition.t;
if(curpos) *curpos = pt;
return TRUE;
}
// X=X0+1'/s, Y=Y0+15''/s
int Linear(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + ASEC2RAD(0.1) * (t - tstart);
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
if(nextpt) *nextpt = pt;
return TRUE;
}
// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
int SinCos(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + ASEC2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
pt.Y = XYstart.Y + AMIN2RAD(1.)* cos((t-tstart)/200.*2*M_PI);
if(nextpt) *nextpt = pt;
return TRUE;
}
typedef struct{
traectory_fn f;
const char *name;
const char *help;
} tr_names;
static tr_names names[] = {
{Linear, "linear", "X=X0+0.1''/s, Y=Y0+15''/s"},
{SinCos, "sincos", "X=X0+5''*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
{NULL, NULL, NULL}
};
traectory_fn traectory_by_name(const char *name){
traectory_fn f = NULL;
for(int i = 0; ; ++i){
if(!names[i].f) break;
if(strcmp(names[i].name, name) == 0){
f = names[i].f;
break;
}
}
return f;
}
// print all acceptable traectories names with help
void print_tr_names(){
for(int i = 0; ; ++i){
if(!names[i].f) break;
printf("%s: %s\n", names[i].name, names[i].help);
}
}

32
examples/traectories.h Normal file
View File

@@ -0,0 +1,32 @@
/*
* This file is part of the libsidservo project.
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "sidservo.h"
// traectory
typedef int (*traectory_fn)(coordpair_t *, double);
int init_traectory(traectory_fn f, coordpair_t *XY0);
traectory_fn traectory_by_name(const char *name);
void print_tr_names();
int traectory_point(coordpair_t *nextpt, double t);
int telpos(coordval_pair_t *curpos);
int Linear(coordpair_t *nextpt, double t);
int SinCos(coordpair_t *nextpt, double t);