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

add speed + a lot of refactoring

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-07-22 23:52:08 +03:00
parent fc943abe03
commit b0421972d1
22 changed files with 513 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
LibSidServo/examples/conf.c
View File

@@ -29,7 +29,8 @@ static conf_t Config = {
.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.EncoderReqInterval = 0.05,
.SepEncoder = 2
.SepEncoder = 2,
.EncoderSpeedInterval = 0.1,
};
static sl_option_t opts[] = {
@@ -42,6 +43,7 @@ static sl_option_t opts[] = {
{"SepEncoder", NO_ARGS, 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)"},
{"EncoderSpeedInterval", NEED_ARG,NULL, 0, arg_double, APTR(&Config.EncoderSpeedInterval),"interval of speed calculations, s"},
end_option
};

99
LibSidServo/examples/dump.c
View File

@@ -23,6 +23,42 @@
#include "dump.h"
#include "simpleconv.h"
#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
/**
* @brief logmnt - log mount data into file
* @param fcoords - file to dump
@@ -33,16 +69,18 @@ void logmnt(FILE *fcoords, mountdata_t *m){
//DBG("LOG %s", m ? "data" : "header");
static double t0 = -1.;
if(!m){ // write header
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) millis T V\n");
fprintf(fcoords, "# time Xmot(deg) Ymot(deg) Xenc(deg) Yenc(deg) VX(d/s) VY(d/s) millis\n");
return;
}
if(t0 < 0.) t0 = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6;
double t = m->encposition.msrtime.tv_sec + (double)(m->encposition.msrtime.tv_usec) / 1e6 - t0;
double tnow = (m->encXposition.t + m->encYposition.t) / 2.;
if(t0 < 0.) t0 = tnow;
double t = tnow - 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);
fprintf(fcoords, "%12.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10u\n",
t, 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);
}
@@ -64,21 +102,24 @@ void dumpmoving(FILE *fcoords, double t, int N){
WARNX("Can't get mount data");
LOGWARN("Can't get mount data");
}
uint32_t millis = mdata.encposition.msrtime.tv_usec;
uint32_t mdmillis = mdata.millis;
double enct = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
int ctr = -1;
double xlast = mdata.motposition.X, ylast = mdata.motposition.Y;
double t0 = sl_dtime();
//DBG("millis = %u", millis);
while(sl_dtime() - t0 < t && ctr < N){
double xlast = mdata.motXposition.val, ylast = mdata.motYposition.val;
double t0 = Mount.currentT();
while(Mount.currentT() - t0 < t && ctr < N){
usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.encposition.msrtime.tv_usec == millis) continue;
//DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y);
millis = mdata.encposition.msrtime.tv_usec;
double tmsr = (mdata.encXposition.t + mdata.encYposition.t) / 2.;
if(tmsr == enct) continue;
enct = tmsr;
if(fcoords) logmnt(fcoords, &mdata);
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
xlast = mdata.motposition.X;
ylast = mdata.motposition.Y;
if(mdata.millis == mdmillis) continue;
DBG("ctr=%d", ctr);
mdmillis = mdata.millis;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
ctr = 0;
}else ++ctr;
}
@@ -98,9 +139,9 @@ void waitmoving(int N){
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
xlast = mdata.motposition.X;
ylast = mdata.motposition.Y;
if(mdata.motXposition.val != xlast || mdata.motYposition.val != ylast){
xlast = mdata.motXposition.val;
ylast = mdata.motYposition.val;
ctr = 0;
}else ++ctr;
}
@@ -112,7 +153,7 @@ void waitmoving(int N){
* @param Y (o) - encoder position (or NULL)
* @return FALSE if failed
*/
int getPos(coords_t *mot, coords_t *enc){
int getPos(coordval_pair_t *mot, coordval_pair_t *enc){
mountdata_t mdata = {0};
int errcnt = 0;
do{
@@ -126,16 +167,22 @@ int getPos(coords_t *mot, coords_t *enc){
WARNX("Can't read mount status");
return FALSE;
}
if(mot) *mot = mdata.motposition;
if(enc) *enc = mdata.encposition;
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){
coords_t M;
coordval_pair_t M;
if(!getPos(&M, NULL)) signals(2);
if(M.X || M.Y){
if(M.X.val || M.Y.val){
WARNX("Mount position isn't @ zero; moving");
double zero = 0.;
Mount.moveTo(&zero, &zero);

2
LibSidServo/examples/dump.h
View File

@@ -25,5 +25,5 @@
void logmnt(FILE *fcoords, mountdata_t *m);
void dumpmoving(FILE *fcoords, double t, int N);
void waitmoving(int N);
int getPos(coords_t *mot, coords_t *enc);
int getPos(coordval_pair_t *mot, coordval_pair_t *enc);
void chk0(int ncycles);

6
LibSidServo/examples/dumpmoving.c
View File

@@ -85,18 +85,18 @@ int main(int argc, char **argv){
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");
coords_t M;
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
double tagx = DEG2RAD(45.) + M.X, tagy = DEG2RAD(45.) + M.Y;
double tagx = DEG2RAD(45.) + M.X.val, tagy = DEG2RAD(45.) + M.Y.val;
if(MCC_E_OK != Mount.moveTo(&tagx, &tagy))
ERRX("Can't move to 45, 45");
dumpmoving(fcoords, 30., G.Ncycles);
Mount.moveTo(&M.X, &M.Y);
Mount.moveTo(&M.X.val, &M.Y.val);
dumpmoving(fcoords, 30., G.Ncycles);
signals(0);
return 0;

170
LibSidServo/examples/dumpmoving_dragNtrack.c Normal file
View File

@@ -0,0 +1,170 @@
/*
* 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
#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 coords_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.motposition.X;
else curpos = mdata.motposition.Y;
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;
cmd.Xspeed = DEG2RAD(speed);
limit = DEG2RAD(limit) + M.X;
}
if(*G.axis == 'Y' || *G.axis == 'B'){
cmd.Ymot = DEG2RAD(target) + M.Y;
cmd.Yspeed = DEG2RAD(speed);
if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y;
}
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 10 degr with 2d/s and try to track for 8 seconds
move(10., 10.+2./60., 2.);
// be sure to move @ 0,0
Mount.moveTo(&M.X, &M.Y);
// wait moving ends
pthread_join(dthr, NULL);
signals(0);
return 0;
}

23
LibSidServo/examples/dumpmoving_scmd.c
View File

@@ -49,7 +49,7 @@ static parameters G = {
static FILE *fcoords = NULL;
static coords_t M;
static coordval_pair_t M;
static sl_option_t cmdlnopts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
@@ -92,8 +92,8 @@ static int Wait(double tag){
errcnt = 0;
if(mdata.millis == millis) continue;
millis = mdata.millis;
if(*G.axis == 'X') curpos = mdata.motposition.X;
else curpos = mdata.motposition.Y;
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.);
@@ -110,22 +110,20 @@ static int Wait(double tag){
// move X/Y to 40 degr with given speed until given coord
static void move(double target, double limit, double speed){
#define SCMD() do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0)
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;
cmd.Xmot = DEG2RAD(target) + M.X.val;
cmd.Xspeed = DEG2RAD(speed);
limit = DEG2RAD(limit) + M.X;
limit = DEG2RAD(limit) + M.X.val;
}
if(*G.axis == 'Y' || *G.axis == 'B'){
cmd.Ymot = DEG2RAD(target) + M.Y;
cmd.Ymot = DEG2RAD(target) + M.Y.val;
cmd.Yspeed = DEG2RAD(speed);
if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y;
if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y.val;
}
SCMD();
if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command");
if(!Wait(limit)) signals(9);
#undef SCMD
}
@@ -169,11 +167,10 @@ int main(int argc, char **argv){
move(10., 3., 15./60.);
// and go back with 7deg/s
move(0., 0., 7.);
// be sure to move @ 0,0
Mount.moveTo(&M.X, &M.Y);
// be sure to move @ starting position
Mount.moveTo(&M.X.val, &M.Y.val);
// wait moving ends
pthread_join(dthr, NULL);
#undef SCMD
signals(0);
return 0;
}

20
LibSidServo/examples/dumpswing.c
View File

@@ -83,18 +83,18 @@ void waithalf(double t){
uint32_t millis = 0;
double xlast = 0., ylast = 0.;
while(ctr < 5){
if(sl_dtime() >= t) return;
if(Mount.currentT() >= 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.motposition.X != xlast || mdata.motposition.Y != ylast){
DBG("NEQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y));
xlast = mdata.motposition.X;
ylast = mdata.motposition.Y;
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.motposition.Y));
DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motYposition.val));
++ctr;
}
}
@@ -145,21 +145,21 @@ int main(int argc, char **argv){
}else{
tagX = 0.; tagY = DEG2RAD(G.amplitude);
}
double t = sl_dtime(), t0 = t;
double t = Mount.currentT(), t0 = t;
double divide = 2., rtagX = -tagX, rtagY = -tagY;
for(int i = 0; i < G.Nswings; ++i){
Mount.moveTo(&tagX, &tagY);
DBG("CMD: %g", sl_dtime()-t0);
DBG("CMD: %g", Mount.currentT()-t0);
t += G.period / divide;
divide = 1.;
waithalf(t);
DBG("Moved to +, t=%g", t-t0);
DBG("CMD: %g", sl_dtime()-t0);
DBG("CMD: %g", Mount.currentT()-t0);
Mount.moveTo(&rtagX, &rtagY);
t += G.period;
waithalf(t);
DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", sl_dtime()-t0);
DBG("CMD: %g", Mount.currentT()-t0);
}
double zero = 0.;
// be sure to move @ 0,0

16
LibSidServo/examples/goto.c
View File

@@ -40,7 +40,7 @@ typedef struct{
} parameters;
static parameters G = {
.Ncycles = 40,
.Ncycles = 10,
.X = NAN,
.Y = NAN,
};
@@ -88,7 +88,7 @@ int main(int _U_ argc, char _U_ **argv){
return 1;
}
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
coords_t M;
coordval_pair_t M;
if(!getPos(&M, NULL)) ERRX("Can't get current position");
if(G.coordsoutput){
if(!G.wait) green("When logging I should wait until moving ends; added '-w'");
@@ -100,20 +100,20 @@ int main(int _U_ argc, char _U_ **argv){
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
}
printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
if(isnan(G.X) && isnan(G.Y)) goto out;
double *xtag = NULL, *ytag = NULL, xr, yr;
double _7deg = RAD2DEG(7.);
if(!isnan(G.X)){
xr = DEG2RAD(G.X);
if(G.relative) xr += M.X;
if(G.relative) xr += M.X.val;
xtag = &xr;
// set max speed
Mount.setSpeed(&_7deg, NULL);
}
if(!isnan(G.Y)){
yr = DEG2RAD(G.Y);
if(G.relative) yr += M.Y;
if(G.relative) yr += M.Y.val;
ytag = &yr;
Mount.setSpeed(NULL, &_7deg);
}
@@ -126,12 +126,14 @@ int main(int _U_ argc, char _U_ **argv){
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), RAD2DEG(M.Y));
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){
if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val));
Mount.quit();
}
return 0;

19
LibSidServo/examples/scmd_traectory.c
View File

@@ -89,20 +89,21 @@ static void *dumping(void _U_ *u){
// calculate
static void runtraectory(traectory_fn tfn){
if(!tfn) return;
coords_t telXY, traectXY;
double t0 = sl_dtime();
uint32_t susec_last = 0;
coordval_pair_t telXY;
coordpair_t traectXY;
double t0 = Mount.currentT();
double tlast = 0.;
while(1){
if(!telpos(&telXY)){
WARNX("No next telescope position");
return;
}
if(telXY.msrtime.tv_usec == susec_last) continue; // last measure - don't mind
susec_last = telXY.msrtime.tv_usec;
double t = sl_dtime();
if(telXY.X > G.Xmax || telXY.Y > G.Ymax || t - t0 > G.tmax) break;
if(telXY.X.t == tlast && telXY.Y.t == tlast) continue; // last measure - don't mind
tlast = (telXY.X.t + telXY.Y.t) / 2.;
double t = Mount.currentT();
if(telXY.X.val > G.Xmax || telXY.Y.val > G.Ymax || t - t0 > G.tmax) break;
if(!traectory_point(&traectXY, t)) break;
DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X), RAD2ASEC(traectXY.Y-telXY.Y));
DBG("%g: dX=%.1f'', dY=%.1f''", t-t0, RAD2ASEC(traectXY.X-telXY.X.val), RAD2ASEC(traectXY.Y-telXY.Y.val));
}
WARNX("No next traectory point");
}
@@ -133,7 +134,7 @@ int main(int argc, char **argv){
print_tr_names();
return 1;
}
coords_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
coordpair_t c = {.X = DEG2RAD(G.X0), .Y = DEG2RAD(G.Y0)};
if(!init_traectory(tfn, &c)){
ERRX("Can't init traectory");
return 1;

3
LibSidServo/examples/servo.conf
View File

@@ -4,5 +4,6 @@ EncoderXDevPath=/dev/encoder_X0
EncoderYDevPath=/dev/encoder_Y0
MountReqInterval=0.05
SepEncoder=2
EncoderReqInterval=0.01
EncoderReqInterval=0.001
EncoderDevSpeed=1000000
EncoderSpeedInterval=0.05

39
LibSidServo/examples/traectories.c
View File

@@ -28,12 +28,12 @@
static traectory_fn cur_traectory = NULL;
// starting point of traectory
static coords_t XYstart = {0};
static coordpair_t XYstart = {0};
static double tstart = 0.;
// convert Xe/Ye to approximate motor coordinates:
// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have
// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0
static coords_t XYcor = {0};
static coordval_pair_t XYcor = {0};
/**
* @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
@@ -41,20 +41,20 @@ static coords_t XYcor = {0};
* @param XY0 - starting point
* @return FALSE if failed
*/
int init_traectory(traectory_fn f, coords_t *XY0){
int init_traectory(traectory_fn f, coordpair_t *XY0){
if(!f || !XY0) return FALSE;
cur_traectory = f;
XYstart = *XY0;
tstart = sl_dtime();
tstart = Mount.currentT();
mountdata_t mdata;
int ntries = 0;
for(; ntries < 10; ++ntries){
if(MCC_E_OK == Mount.getMountData(&mdata)) break;
}
if(ntries == 10) return FALSE;
XYcor.X = mdata.motposition.X - mdata.encposition.X;
XYcor.Y = mdata.motposition.X + mdata.encposition.Y;
DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X), DEG2RAD(XYcor.Y));
XYcor.X.val = mdata.motXposition.val - mdata.encXposition.val;
XYcor.Y.val = mdata.motYposition.val - mdata.encYposition.val;
DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X.val), DEG2RAD(XYcor.Y.val));
return TRUE;
}
@@ -64,12 +64,10 @@ int init_traectory(traectory_fn f, coords_t *XY0){
* @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(coords_t *nextpt, double t){
int traectory_point(coordpair_t *nextpt, double t){
if(t < 0. || !cur_traectory) return FALSE;
coords_t pt;
coordpair_t pt;
if(!cur_traectory(&pt, t)) return FALSE;
pt.msrtime.tv_sec = floor(t);
pt.msrtime.tv_usec = (uint32_t) t - pt.msrtime.tv_sec;
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;
@@ -77,24 +75,25 @@ int traectory_point(coords_t *nextpt, double t){
// current telescope position according to starting motor coordinates
// @return FALSE if failed to get current coordinates
int telpos(coords_t *curpos){
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;
coords_t pt;
pt.X = XYcor.X + mdata.encposition.X;
pt.Y = XYcor.Y + mdata.encposition.Y;
pt.msrtime = mdata.encposition.msrtime;
coordval_pair_t pt;
pt.X.val = XYcor.X.val + mdata.encXposition.val;
pt.Y.val = XYcor.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(coords_t *nextpt, double t){
coords_t pt;
int Linear(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
if(nextpt) *nextpt = pt;
@@ -102,8 +101,8 @@ int Linear(coords_t *nextpt, double t){
}
// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
int SinCos(coords_t *nextpt, double t){
coords_t pt;
int SinCos(coordpair_t *nextpt, double t){
coordpair_t pt;
pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
if(nextpt) *nextpt = pt;

12
LibSidServo/examples/traectories.h
View File

@@ -21,12 +21,12 @@
#include "sidservo.h"
// traectory
typedef int (*traectory_fn)(coords_t *, double);
typedef int (*traectory_fn)(coordpair_t *, double);
int init_traectory(traectory_fn f, coords_t *XY0);
int init_traectory(traectory_fn f, coordpair_t *XY0);
traectory_fn traectory_by_name(const char *name);
void print_tr_names();
int traectory_point(coords_t *nextpt, double t);
int telpos(coords_t *curpos);
int Linear(coords_t *nextpt, double t);
int SinCos(coords_t *nextpt, double t);
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);