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

add new encoders' controller

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-04-08 22:47:46 +03:00
parent 16dd3239de
commit 8ca8183cb8
8 changed files with 183 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
LibSidServo/examples/conf.c
View File

@ -24,10 +24,12 @@
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
.EncoderDevPath = "/dev/ttyUSB1",
.EncoderXDevPath = "/dev/encoderX0",
.EncoderYDevPath = "/dev/encoderY0",
.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 1
.EncoderReqInterval = 0.05,
.SepEncoder = 2
};
static sl_option_t opts[] = {
@ -36,7 +38,10 @@ static sl_option_t opts[] = {
{"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"},
{"MountReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.MountReqInterval), "interval of mount requests (not less than 0.05s)"},
{"SepEncoder", NO_ARGS, NULL, 0, arg_int, APTR(&Config.SepEncoder), "encoder is separate device"},
{"EncoderReqInterval",NEED_ARG, NULL, 0, arg_double, APTR(&Config.EncoderReqInterval),"interval of encoder requests (in case of sep=2)"},
{"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)"},
end_option
};

25
LibSidServo/examples/dumpmoving_scmd.c
View File

@ -77,8 +77,8 @@ static void *dumping(void _U_ *u){
return NULL;
}
// return TRUE if motor position is reached +- 0.1 degrees
#define XYcount (DEG2RAD(0.1))
// 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));
@ -104,6 +104,7 @@ static int Wait(double tag){
return FALSE;
}
green("%s reached position %g degrees\n", G.axis, RAD2DEG(tag));
fflush(stdout);
return TRUE;
}
@ -120,7 +121,7 @@ static void move(double target, double limit, double speed){
if(*G.axis == 'Y' || *G.axis == 'B'){
cmd.Ymot = DEG2RAD(target) + M.Y;
cmd.Yspeed = DEG2RAD(speed);
limit = DEG2RAD(limit) + M.Y;
if(*G.axis != 'B') limit = DEG2RAD(limit) + M.Y;
}
SCMD();
if(!Wait(limit)) signals(9);
@ -160,16 +161,14 @@ int main(int argc, char **argv){
pthread_t dthr;
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
// goto 1 degr with 1'/s
move(10., 1., 1./60.);
// goto 2 degr with 2'/s
move(10., 2., 2./60.);
// goto 3 degr with 5'/s
move(10., 3., 5./60.);
// goto 4 degr with 10'/s
move(10., 4., 10./60.);
// and go back with 5deg/s
move(0., 0., 5.);
// 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 @ 0,0
Mount.moveTo(&M.X, &M.Y);
// wait moving ends

4
LibSidServo/examples/goto.c
View File

@ -103,15 +103,19 @@ int main(int _U_ argc, char _U_ **argv){
printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
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;
xtag = &xr;
// set max speed
Mount.setSpeed(&_7deg, NULL);
}
if(!isnan(G.Y)){
yr = DEG2RAD(G.Y);
if(G.relative) yr += M.Y;
ytag = &yr;
Mount.setSpeed(NULL, &_7deg);
}
printf("Moving to ");
if(xtag) printf("X=%gdeg ", G.X);

8
LibSidServo/examples/servo.conf Normal file
View File

@ -0,0 +1,8 @@
MountDevPath=/dev/ttyUSB0
MountDevSpeed=19200
EncoderXDevPath=/dev/encoder_X0
EncoderYDevPath=/dev/encoder_Y0
MountReqInterval=0.05
SepEncoder=2
EncoderReqInterval=0.01
EncoderDevSpeed=1000000

15
LibSidServo/main.c
View File

@ -49,16 +49,16 @@ static mcc_errcodes_t init(conf_t *c){
if(!Conf.MountDevPath || Conf.MountDevSpeed < 1200){
DBG("Define mount device path and speed");
ret = MCC_E_BADFORMAT;
}else if(!openMount(Conf.MountDevPath, Conf.MountDevSpeed)){
}else if(!openMount()){
DBG("Can't open %s with speed %d", Conf.MountDevPath, Conf.MountDevSpeed);
ret = MCC_E_MOUNTDEV;
}
if(Conf.SepEncoder){
if(!Conf.EncoderDevPath || Conf.EncoderDevSpeed < 1200){
DBG("Define encoder device path and speed");
if(!Conf.EncoderDevPath && !Conf.EncoderXDevPath){
DBG("Define encoder device path");
ret = MCC_E_BADFORMAT;
}else if(!openEncoder(Conf.EncoderDevPath, Conf.EncoderDevSpeed)){
DBG("Can't open %s with speed %d", Conf.EncoderDevPath, Conf.EncoderDevSpeed);
}else if(!openEncoder()){
DBG("Can't open encoder device");
ret = MCC_E_ENCODERDEV;
}
}
@ -152,10 +152,10 @@ static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){
return MCC_E_BADFORMAT;
char buf[128];
int32_t spd = X_RS2MOTSPD(speed->X), tag = X_RAD2MOT(target->X);
snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTX, tag, CMD_MOTXYS, spd);
snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, CMD_MOTX, tag, CMD_MOTXYS, spd);
if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
spd = Y_RS2MOTSPD(speed->Y); tag = Y_RAD2MOT(target->Y);
snprintf(buf, 127, "%s%" PRIi64 "%s%" PRIi64, CMD_MOTY, tag, CMD_MOTXYS, spd);
snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, CMD_MOTY, tag, CMD_MOTXYS, spd);
if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
return MCC_E_OK;
}
@ -342,6 +342,7 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
// Convert backlash speed (rad/s to ticks per loop)
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
// TODO - next
(void) config;
return MCC_E_OK;
}

160
LibSidServo/serial.c
View File

@ -34,7 +34,7 @@
#include "serial.h"
// serial devices FD
static int encfd = -1, mntfd = -1;
static int encfd[2] = {-1, -1}, mntfd = -1;
// main mount data
static mountdata_t mountdata = {0};
@ -122,24 +122,72 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
//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);
}
/**
* @brief getencval - get uint64_t data from encoder
* @param fd - encoder fd
* @param val - value read
* @param tv - measurement time
* @return amount of data read or 0 if problem
*/
static int getencval(int fd, double *val, struct timeval *tv){
if(fd < 0) return FALSE;
char buf[128];
int got = 0, Lmax = 127;
double t0 = dtime();
do{
fd_set rfds;
FD_ZERO(&rfds);
FD_SET(fd, &rfds);
struct timeval tv = encRtmout;
int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
if(!retval) continue;
if(retval < 0){
if(errno == EINTR) continue;
return 0;
}
if(FD_ISSET(fd, &rfds)){
ssize_t l = read(fd, &buf[got], Lmax);
if(l < 1) return 0; // disconnected ??
got += l; Lmax -= l;
buf[got] = 0;
} else continue;
if(strchr(buf, '\n')) break;
}while(Lmax && dtime() - t0 > Conf.EncoderReqInterval);
if(got == 0) return 0; // WTF?
char *estr = strrchr(buf, '\n');
if(!estr) return 0;
*estr = 0;
char *bgn = strrchr(buf, '\n');
if(bgn) ++bgn;
else bgn = buf;
char *eptr;
long data = strtol(bgn, &eptr, 10);
if(eptr != estr){
DBG("NAN");
return 0; // wrong number
}
if(val) *val = (double) data;
if(tv) gettimeofday(tv, NULL);
return got;
}
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
static int getencbyte(){
if(encfd < 0) return -1;
if(encfd[0] < 0) return -1;
uint8_t byte = 0;
fd_set rfds;
struct timeval tv;
do{
FD_ZERO(&rfds);
FD_SET(encfd, &rfds);
FD_SET(encfd[0], &rfds);
tv = encRtmout;
int retval = select(encfd + 1, &rfds, NULL, NULL, &tv);
int retval = select(encfd[0] + 1, &rfds, NULL, NULL, &tv);
if(!retval) break;
if(retval < 0){
if(errno == EINTR) continue;
return -1;
}
if(FD_ISSET(encfd, &rfds)){
ssize_t l = read(encfd, &byte, 1);
if(FD_ISSET(encfd[0], &rfds)){
ssize_t l = read(encfd[0], &byte, 1);
if(l != 1) return -2; // disconnected ??
break;
} else return -1;
@ -179,11 +227,12 @@ static int getmntbyte(){
}
// main encoder thread (for separate encoder): read next data and make parsing
static void *encoderthread(void _U_ *u){
static void *encoderthread1(void _U_ *u){
if(Conf.SepEncoder != 1) return NULL;
uint8_t databuf[ENC_DATALEN];
int wridx = 0, errctr = 0;
struct timeval tv;
while(encfd > -1 && errctr < MAX_ERR_CTR){
while(encfd[0] > -1 && errctr < MAX_ERR_CTR){
int b = getencbyte();
if(b == -2) ++errctr;
if(b < 0) continue;
@ -202,9 +251,42 @@ static void *encoderthread(void _U_ *u){
wridx = 0;
}
}
if(encfd > -1){
close(encfd);
encfd = -1;
if(encfd[0] > -1){
close(encfd[0]);
encfd[0] = -1;
}
return NULL;
}
static void *encoderthread2(void _U_ *u){
if(Conf.SepEncoder != 2) return NULL;
DBG("Thread started");
int errctr = 0;
double t0 = dtime();
const char *req = "next\n";
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
if(5 != write(encfd[0], req, 5)) ++errctr;
if(5 != write(encfd[1], req, 5)) ++errctr;
double v;
struct timeval tv;
if(getencval(encfd[0], &v, &tv)){
mountdata.encposition.X = X_ENC2RAD(v);
mountdata.encposition.msrtime = tv;
if(getencval(encfd[1], &v, &tv)){
mountdata.encposition.Y = Y_ENC2RAD(v);
mountdata.encposition.msrtime = tv;
errctr = 0;
} else ++errctr;
} else ++errctr;
while(dtime() - t0 < Conf.EncoderReqInterval){ usleep(10); }
//DBG("DT=%g (RI=%g)", dtime()-t0, Conf.EncoderReqInterval);
t0 = dtime();
}
for(int i = 0; i < 2; ++i){
if(encfd[i] > -1){
close(encfd[i]);
encfd[i] = -1;
}
}
return NULL;
}
@ -293,7 +375,7 @@ static int ttyopen(const char *path, speed_t speed){
//tty.c_cc[VMIN] = 0; // non-canonical mode
//tty.c_cc[VTIME] = 5;
if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; }
DBG("Check speed");
DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed);
if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
// try to set exclusive
if(ioctl(fd, TIOCEXCL)){DBG("Can't make exclusive");}
@ -301,27 +383,47 @@ static int ttyopen(const char *path, speed_t speed){
}
// return FALSE if failed
int openEncoder(const char *path, int speed){
int openEncoder(){
if(!Conf.SepEncoder) return FALSE; // try to open separate encoder when it's absent
if(encfd > -1) close(encfd);
encfd = ttyopen(path, (speed_t) speed);
if(encfd < 0) return FALSE;
if(Conf.SepEncoder == 1){ // only one device
DBG("One device");
if(encfd[0] > -1) close(encfd[0]);
encfd[0] = ttyopen(Conf.EncoderDevPath, (speed_t) Conf.EncoderDevSpeed);
if(encfd[0] < 0) return FALSE;
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 200000000 / speed; // 20 bytes
if(pthread_create(&encthread, NULL, encoderthread, NULL)){
close(encfd);
encfd = -1;
encRtmout.tv_usec = 200000000 / Conf.EncoderDevSpeed; // 20 bytes
if(pthread_create(&encthread, NULL, encoderthread1, NULL)){
close(encfd[0]);
encfd[0] = -1;
return FALSE;
}
}else if(Conf.SepEncoder == 2){
DBG("Two devices!");
const char* paths[2] = {Conf.EncoderXDevPath, Conf.EncoderYDevPath};
for(int i = 0; i < 2; ++i){
if(encfd[i] > -1) close(encfd[i]);
encfd[i] = ttyopen(paths[i], (speed_t) Conf.EncoderDevSpeed);
if(encfd[i] < 0) return FALSE;
}
encRtmout.tv_sec = 0;
encRtmout.tv_usec = 1000; // 1ms
if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
for(int i = 0; i < 2; ++i){
close(encfd[i]);
encfd[i] = -1;
}
return FALSE;
}
}else return FALSE;
DBG("Encoder opened, thread started");
return TRUE;
}
// return FALSE if failed
int openMount(const char *path, int speed){
int openMount(){
if(mntfd > -1) close(mntfd);
DBG("Open mount %s @ %d", path, speed);
mntfd = ttyopen(path, (speed_t) speed);
DBG("Open mount %s @ %d", Conf.MountDevPath, Conf.MountDevSpeed);
mntfd = ttyopen(Conf.MountDevPath, (speed_t) Conf.MountDevSpeed);
if(mntfd < 0) return FALSE;
DBG("mntfd=%d", mntfd);
// clear buffer
@ -334,7 +436,7 @@ int openMount(const char *path, int speed){
DBG("got %zd", l);
}while(1);*/
mntRtmout.tv_sec = 0;
mntRtmout.tv_usec = 500000000 / speed; // 50 bytes
mntRtmout.tv_usec = 500000000 / Conf.MountDevSpeed; // 50 bytes
if(pthread_create(&mntthread, NULL, mountthread, NULL)){
DBG("Can't create thread");
close(mntfd);
@ -354,12 +456,16 @@ void closeSerial(){
close(mntfd);
mntfd = -1;
}
if(encfd > -1){
if(encfd[0] > -1){
DBG("Kill encoder thread");
pthread_cancel(encthread);
DBG("close fd");
close(encfd);
encfd = -1;
close(encfd[0]);
encfd[0] = -1;
if(Conf.SepEncoder == 2){
close(encfd[1]);
encfd[1] = -1;
}
}
}

4
LibSidServo/serial.h
View File

@ -31,8 +31,8 @@
double dtime();
data_t *cmd2dat(const char *cmd);
void data_free(data_t **x);
int openEncoder(const char *path, int speed);
int openMount(const char *path, int speed);
int openEncoder();
int openMount();
void closeSerial();
mcc_errcodes_t getMD(mountdata_t *d);
int MountWriteRead(const data_t *out, data_t *in);

7
LibSidServo/sidservo.h
View File

@ -42,8 +42,11 @@ typedef struct{
int MountDevSpeed; // serial speed
char* EncoderDevPath; // path to encoder device
int EncoderDevSpeed; // serial speed
int SepEncoder; // ==1 if encoder works as separate serial device
double MountReqInterval; // maximal interval between subsequent mount requests (seconds)
int SepEncoder; // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
char* EncoderXDevPath; // paths to new controller devices
char* EncoderYDevPath;
double MountReqInterval; // interval between subsequent mount requests (seconds)
double EncoderReqInterval; // interval between subsequent encoder requests (seconds)
;
} conf_t;