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

nxt

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-02-08 19:48:44 +03:00
parent 55d8d359b4
commit d7df8e5bf1
8 changed files with 387 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
LibSidServo/examples/dumpmoving.c
View File

@@ -16,6 +16,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <math.h>
#include <signal.h>
#include <stdio.h>
#include <time.h>
@@ -31,6 +32,7 @@ typedef struct{
} parameters;
static parameters G = {0};
static FILE *fcoords = NULL;
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
@@ -51,13 +53,67 @@ void signals(int sig){
}
static conf_t Config = {
.MountDevPath = "/dev/ttyS1",
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB0",
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 0
};
#define DEG2RAD(d) (d/180.*M_PI)
#define RAD2DEG(d) (d/M_PI*180.)
static void logmnt(mountdata_t *m){
DBG("LOG");
static double t0 = -1.;
if(!fcoords) return;
if(!m){ // write header
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n");
return;
}
if(t0 < 0.) t0 = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6;
double t = m->motposition.msrtime.tv_sec + (double)(m->motposition.msrtime.tv_usec) / 1e6 - t0;
// write data
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10u %6.1f %4.1f\n",
t, RAD2DEG(m->motposition.X), RAD2DEG(m->motposition.Y),
RAD2DEG(m->encposition.X), RAD2DEG(m->encposition.Y),
m->millis, m->temperature, m->voltage);
}
// dump moving but not longer than t secs; exit after 5 stopped events
static void dumpmoving(double t){
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");
}
suseconds_t usec = mdata.motposition.msrtime.tv_usec;
int ctr = 0;
double xlast = mdata.motposition.X, ylast = mdata.motposition.Y;
double t0 = sl_dtime();
DBG("usec = %ld", usec);
while(sl_dtime() - t0 < t && ctr < 5){
usleep(10000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.motposition.msrtime.tv_usec == usec) continue;
DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
usec = mdata.motposition.msrtime.tv_usec;
if(fcoords) logmnt(&mdata);
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
xlast = mdata.motposition.X;
ylast = mdata.motposition.Y;
ctr = 0;
}else ++ctr;
}
}
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
@@ -65,18 +121,30 @@ int main(int argc, char **argv){
sl_loglevel_e lvl = G.verbose + LOGLEVEL_ERR;
if(lvl >= LOGLEVEL_AMOUNT) lvl = LOGLEVEL_AMOUNT - 1;
if(G.logfile) OPENLOG(G.logfile, lvl, 1);
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
logmnt(NULL);
}
time_t curtime = time(NULL);
LOGMSG("Started @ %s", ctime(&curtime));
DBG("Devices ready");
LOGMSG("Mount device %s @ %d", Config.MountDevPath, Config.MountDevSpeed);
LOGMSG("Encoder device %s @ %d", Config.EncoderDevPath, Config.EncoderDevSpeed);
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
double t0 = sl_dtime();
while(sl_dtime() - t0 < 10.);
Mount.quit();
if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.)))
ERRX("Can't move to 45, 45");
dumpmoving(30.);
Mount.moveTo(0., 0.);
dumpmoving(30.);
signals(0);
return 0;
}