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

fixed race bug

Browse Source
This commit is contained in:
Edward V. Emelianov 2026-01-21 23:20:58 +03:00
parent f7cb279841
commit 873f292a11
7 changed files with 115 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LibSidServo/.qtcreator/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 18.0.1, 2026-01-20T23:16:00. --> <!-- Written by QtCreator 18.0.1, 2026-01-21T23:20:23. -->
<qtcreator> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>

3
LibSidServo/TODO
View File

@ -1,2 +1,3 @@
check angle conversions fix encoders opening for several tries
encoderthread2() - change main cycle (remove pause, read data independently, ask for new only after timeout after last request)
Read HW config even in model mode Read HW config even in model mode

2
LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 18.0.1, 2026-01-20T23:16:00. --> <!-- Written by QtCreator 18.0.1, 2026-01-21T23:20:23. -->
<qtcreator> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>

12
LibSidServo/main.c
View File

@ -25,6 +25,7 @@
#include <time.h> #include <time.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <unistd.h>
#include "main.h" #include "main.h"
#include "movingmodel.h" #include "movingmodel.h"
@ -225,6 +226,9 @@ static mcc_errcodes_t init(conf_t *c){
// read HW config to update constants // read HW config to update constants
hardware_configuration_t HW; hardware_configuration_t HW;
if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED; if(MCC_E_OK != get_hwconf(&HW)) return MCC_E_FAILED;
// make a pause for actual encoder's values
double t0 = timefromstart();
while(timefromstart() - t0 < Conf.EncoderReqInterval) usleep(1000);
return updateMotorPos(); return updateMotorPos();
} }
@ -489,15 +493,19 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
Yticks = ((double) i64) / 4.; Yticks = ((double) i64) / 4.;
X_ENC_ZERO = Conf.XEncZero; X_ENC_ZERO = Conf.XEncZero;
Y_ENC_ZERO = Conf.YEncZero; Y_ENC_ZERO = Conf.YEncZero;
X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = (config.xbits.motrev) ? -Xticks : Xticks; DBG("xyrev: %d/%d", config.xbits.motrev, config.ybits.motrev);
Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = (config.ybits.motrev) ? -Yticks : Yticks; X_MOT_STEPSPERREV = hwConfig->Xconf.motor_stepsperrev = Xticks; // (config.xbits.motrev) ? -Xticks : Xticks;
Y_MOT_STEPSPERREV = hwConfig->Yconf.motor_stepsperrev = Yticks; //(config.ybits.motrev) ? -Yticks : Yticks;
DBG("zero: %d/%d; motsteps: %.10g/%.10g", X_ENC_ZERO, Y_ENC_ZERO, X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV);
// axis encoder ticks per rev // axis encoder ticks per rev
if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED; if(!SSgetint(CMD_AEPRX, &i64)) return MCC_E_FAILED;
Xticks = (double) i64; Xticks = (double) i64;
if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED; if(!SSgetint(CMD_AEPRY, &i64)) return MCC_E_FAILED;
Yticks = (double) i64; Yticks = (double) i64;
DBG("xyencrev: %d/%d", config.xbits.encrev, config.ybits.encrev);
X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks; X_ENC_STEPSPERREV = hwConfig->Xconf.axis_stepsperrev = (config.xbits.encrev) ? -Xticks : Xticks;
Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks; Y_ENC_STEPSPERREV = hwConfig->Yconf.axis_stepsperrev = (config.ybits.encrev) ? -Yticks : Yticks;
DBG("encsteps: %.10g/%.10g", X_ENC_STEPSPERREV, Y_ENC_STEPSPERREV);
return MCC_E_OK; return MCC_E_OK;
} }

101
LibSidServo/serial.c
View File

@ -48,7 +48,7 @@ static pthread_mutex_t mntmutex = PTHREAD_MUTEX_INITIALIZER,
// encoders thread and mount thread // encoders thread and mount thread
static pthread_t encthread, mntthread; static pthread_t encthread, mntthread;
// max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select` // max timeout for 1.5 bytes of encoder and 2 bytes of mount - for `select`
static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 5000}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000}; static struct timeval encRtmout = {.tv_sec = 0, .tv_usec = 100}, mntRtmout = {.tv_sec = 0, .tv_usec = 50000};
// encoders raw data // encoders raw data
typedef struct __attribute__((packed)){ typedef struct __attribute__((packed)){
uint8_t magick; uint8_t magick;
@ -151,32 +151,52 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
* @return amount of data read or 0 if problem * @return amount of data read or 0 if problem
*/ */
static int getencval(int fd, double *val, struct timespec *t){ static int getencval(int fd, double *val, struct timespec *t){
if(fd < 0) return FALSE; if(fd < 0){
DBG("Encoder fd < 0!");
return FALSE;
}
char buf[128]; char buf[128];
int got = 0, Lmax = 127; int got = 0, Lmax = 127;
double t0 = timefromstart(); double t0 = timefromstart();
//DBG("start: %.6g", t0);
do{ do{
fd_set rfds; fd_set rfds;
FD_ZERO(&rfds); FD_ZERO(&rfds);
FD_SET(fd, &rfds); FD_SET(fd, &rfds);
struct timeval tv = encRtmout; struct timeval tv = encRtmout;
int retval = select(fd + 1, &rfds, NULL, NULL, &tv); int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
if(!retval) continue; if(!retval){
//DBG("select()==0 - timeout, %.6g", timefromstart());
break;
}
if(retval < 0){ if(retval < 0){
if(errno == EINTR) continue; if(errno == EINTR){
DBG("EINTR");
continue;
}
DBG("select() < 0");
return 0; return 0;
} }
if(FD_ISSET(fd, &rfds)){ if(FD_ISSET(fd, &rfds)){
ssize_t l = read(fd, &buf[got], Lmax); ssize_t l = read(fd, &buf[got], Lmax);
if(l < 1) return 0; // disconnected ?? if(l < 1){
DBG("read() < 0");
return 0; // disconnected ??
}
got += l; Lmax -= l; got += l; Lmax -= l;
buf[got] = 0; buf[got] = 0;
} else continue; } else continue;
if(strchr(buf, '\n')) break; if(buf[got-1] == '\n') break; // got EOL as last symbol
}while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval); }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.);
if(got == 0) return 0; // WTF? if(got == 0){
//DBG("No data from encoder, tfs=%.6g", timefromstart());
return 0;
}
char *estr = strrchr(buf, '\n'); char *estr = strrchr(buf, '\n');
if(!estr) return 0; if(!estr){
DBG("No EOL");
return 0;
}
*estr = 0; *estr = 0;
char *bgn = strrchr(buf, '\n'); char *bgn = strrchr(buf, '\n');
if(bgn) ++bgn; if(bgn) ++bgn;
@ -188,7 +208,7 @@ static int getencval(int fd, double *val, struct timespec *t){
return 0; // wrong number return 0; // wrong number
} }
if(val) *val = (double) data; if(val) *val = (double) data;
if(t){ if(!curtime(t)) return 0; } if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }}
return got; return got;
} }
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
@ -312,10 +332,17 @@ static void *encoderthread2(void _U_ *u){
int need2ask = 0; // need or not to ask encoder for new data int need2ask = 0; // need or not to ask encoder for new data
while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){ while(encfd[0] > -1 && encfd[1] > -1 && errctr < MAX_ERR_CTR){
struct timespec t; struct timespec t;
if(!curtime(&t)) continue;
if(need2ask){ if(need2ask){
//DBG("ASK for new data, tfs=%.6g", timefromstart());
if(1 != write(encfd[0], req, 1)) { ++errctr; continue; } if(1 != write(encfd[0], req, 1)) { ++errctr; continue; }
else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; } else if(1 != write(encfd[1], req, 1)) { ++errctr; continue; }
//DBG("OK");
need2ask = 0;
usleep(100);
}
if(!curtime(&t)){
DBG("Where is time?");
continue;
} }
double v; double v;
if(getencval(encfd[0], &v, &t)){ if(getencval(encfd[0], &v, &t)){
@ -323,29 +350,38 @@ static void *encoderthread2(void _U_ *u){
mountdata.encXposition.val = Xenc2rad(v); mountdata.encXposition.val = Xenc2rad(v);
mountdata.encXposition.t = t; mountdata.encXposition.t = t;
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
//DBG("MDXpos: %.10g/%g (xez=%d, xesr=%.10g), tfs=%.6g", v, mountdata.encXposition.val, X_ENC_ZERO, X_ENC_STEPSPERREV, timefromstart());
getXspeed(); getXspeed();
if(getencval(encfd[1], &v, &t)){ if(getencval(encfd[1], &v, &t)){
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
mountdata.encYposition.val = Yenc2rad(v); mountdata.encYposition.val = Yenc2rad(v);
mountdata.encYposition.t = t; mountdata.encYposition.t = t;
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
//DBG("MDYpos: %.10g/%g (yez=%d, yesr=%.10g)", v, mountdata.encYposition.val, Y_ENC_ZERO, Y_ENC_STEPSPERREV);
getYspeed(); getYspeed();
errctr = 0; errctr = 0;
need2ask = 0; need2ask = 0;
} else { //DBG("tgot=%.6g", timefromstart());
if(need2ask) ++errctr;
else need2ask = 1;
continue;
}
} else {
if(need2ask) ++errctr;
else need2ask = 1;
continue;
}
while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); } while(timefromstart() - t0 < Conf.EncoderReqInterval){ usleep(50); }
t0 = timefromstart(); t0 = timefromstart();
}else{
DBG("NO Y DATA!!!");
/*if(need2ask) ++errctr;
else need2ask = 1;
continue;*/
++errctr;
need2ask = 1;
} }
DBG("ERRCTR=%d", errctr); }else{ // no data - ask for new
//DBG("Need new, tfs=%.6g", timefromstart());
/*if(need2ask) ++errctr;
else need2ask = 1;
continue;*/
++errctr;
need2ask = 1;
}
}
DBG("\n\nEXIT: ERRCTR=%d", errctr);
for(int i = 0; i < 2; ++i){ for(int i = 0; i < 2; ++i){
if(encfd[i] > -1){ if(encfd[i] > -1){
close(encfd[i]); close(encfd[i]);
@ -480,8 +516,15 @@ static int ttyopen(const char *path, speed_t speed){
int fd = -1; int fd = -1;
struct termios2 tty; struct termios2 tty;
DBG("Try to open %s @ %d", path, speed); DBG("Try to open %s @ %d", path, speed);
if((fd = open(path, O_RDWR|O_NOCTTY)) < 0) return -1; if((fd = open(path, O_RDWR|O_NOCTTY)) < 0){
if(ioctl(fd, TCGETS2, &tty)){ close(fd); return -1; } DBG("Can't open device %s: %s", path, strerror(errno));
return -1;
}
if(ioctl(fd, TCGETS2, &tty)){
DBG("Can't read TTY settings");
close(fd);
return -1;
}
tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG) tty.c_lflag = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
tty.c_iflag = 0; // don't do any changes in input stream tty.c_iflag = 0; // don't do any changes in input stream
tty.c_oflag = 0; // don't do any changes in output stream tty.c_oflag = 0; // don't do any changes in output stream
@ -490,7 +533,11 @@ static int ttyopen(const char *path, speed_t speed){
tty.c_ospeed = speed; tty.c_ospeed = speed;
//tty.c_cc[VMIN] = 0; // non-canonical mode //tty.c_cc[VMIN] = 0; // non-canonical mode
//tty.c_cc[VTIME] = 5; //tty.c_cc[VTIME] = 5;
if(ioctl(fd, TCSETS2, &tty)){ close(fd); return -1; } if(ioctl(fd, TCSETS2, &tty)){
DBG("Can't set TTY settings");
close(fd);
return -1;
}
DBG("Check speed: i=%d, o=%d", tty.c_ispeed, tty.c_ospeed); 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; } if(tty.c_ispeed != (speed_t) speed || tty.c_ospeed != (speed_t)speed){ close(fd); return -1; }
// try to set exclusive // try to set exclusive
@ -523,7 +570,7 @@ int openEncoder(){
if(encfd[i] < 0) return FALSE; if(encfd[i] < 0) return FALSE;
} }
encRtmout.tv_sec = 0; encRtmout.tv_sec = 0;
encRtmout.tv_usec = 1000; // 1ms encRtmout.tv_usec = 100000000 / Conf.EncoderDevSpeed;
if(pthread_create(&encthread, NULL, encoderthread2, NULL)){ if(pthread_create(&encthread, NULL, encoderthread2, NULL)){
for(int i = 0; i < 2; ++i){ for(int i = 0; i < 2; ++i){
close(encfd[i]); close(encfd[i]);
@ -602,6 +649,8 @@ mcc_errcodes_t getMD(mountdata_t *d){
pthread_mutex_lock(&datamutex); pthread_mutex_lock(&datamutex);
*d = mountdata; *d = mountdata;
pthread_mutex_unlock(&datamutex); pthread_mutex_unlock(&datamutex);
//DBG("ENCpos: %.10g/%.10g", d->encXposition.val, d->encYposition.val);
//DBG("millis: %u, encxt: %zd (time: %zd)", d->millis, d->encXposition.t.tv_sec, time(NULL));
return MCC_E_OK; return MCC_E_OK;
} }

9
LibSidServo/ssii.c
View File

@ -79,6 +79,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, struct timespec *t){
// fill encoder data from here, as there's no separate enc thread // fill encoder data from here, as there's no separate enc thread
if(!Conf.SepEncoder){ if(!Conf.SepEncoder){
m->encXposition.val = Xenc2rad(s->Xenc); m->encXposition.val = Xenc2rad(s->Xenc);
DBG("encx: %g", m->encXposition.val);
m->encYposition.val = Yenc2rad(s->Yenc); m->encYposition.val = Yenc2rad(s->Yenc);
m->encXposition.t = m->encYposition.t = *t; m->encXposition.t = m->encYposition.t = *t;
getXspeed(); getYspeed(); getXspeed(); getYspeed();
@ -188,9 +189,10 @@ mcc_errcodes_t updateMotorPos(){
usleep(10000); usleep(10000);
continue; continue;
} }
//DBG("XENC2RAD: %g (xez=%d, xesr=%.10g)", Xenc2rad(32424842), X_ENC_ZERO, X_ENC_STEPSPERREV);
if(MCC_E_OK == getMD(&md)){ if(MCC_E_OK == getMD(&md)){
if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){ if(md.encXposition.t.tv_sec == 0 || md.encYposition.t.tv_sec == 0){
DBG("Just started, t-t0 = %g!", t - t0); DBG("Just started? t-t0 = %g!", t - t0);
usleep(10000); usleep(10000);
continue; continue;
} }
@ -198,14 +200,15 @@ mcc_errcodes_t updateMotorPos(){
DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec); DBG("got; t pos x/y: %ld/%ld; tnow: %ld", md.encXposition.t.tv_sec, md.encYposition.t.tv_sec, curt.tv_sec);
mcc_errcodes_t OK = MCC_E_OK; mcc_errcodes_t OK = MCC_E_OK;
if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){ if(fabs(md.motXposition.val - md.encXposition.val) > Conf.EncodersDisagreement && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val); DBG("NEED to sync X: motors=%g, axis=%g", md.motXposition.val, md.encXposition.val);
DBG("new motsteps: %d", X_RAD2MOT(md.encXposition.val));
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){ if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
DBG("Xpos sync failed!"); DBG("Xpos sync failed!");
OK = MCC_E_FAILED; OK = MCC_E_FAILED;
}else DBG("Xpos sync OK, Dt=%g", t - t0); }else DBG("Xpos sync OK, Dt=%g", t - t0);
} }
if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){ if(fabs(md.motYposition.val - md.encYposition.val) > Conf.EncodersDisagreement && md.Ystate == AXIS_STOPPED){
DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val); DBG("NEED to sync Y: motors=%g, axis=%g", md.motYposition.val, md.encYposition.val);
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){ if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
DBG("Ypos sync failed!"); DBG("Ypos sync failed!");
OK = MCC_E_FAILED; OK = MCC_E_FAILED;

42
LibSidServo/ssii.h
View File

@ -199,44 +199,46 @@ extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STE
// encoder reversed (no: +1) -> sign of ...stepsperrev // encoder reversed (no: +1) -> sign of ...stepsperrev
//#define X_ENC_SIGN (-1.) //#define X_ENC_SIGN (-1.)
//#define Y_ENC_SIGN (-1.) //#define Y_ENC_SIGN (-1.)
// encoder position to radians and back // encoder position to radians and back
#define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-X_ENC_ZERO)) / X_ENC_STEPSPERREV) #define Xenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(X_ENC_ZERO))) / (X_ENC_STEPSPERREV))
#define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV) #define Yenc2rad(n) ang2half(2.*M_PI * ((double)((n)-(Y_ENC_ZERO))) / (Y_ENC_STEPSPERREV))
#define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV)) #define Xrad2enc(r) ((uint32_t)((r) / 2./M_PI * (X_ENC_STEPSPERREV)))
#define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV)) #define Yrad2enc(r) ((uint32_t)((r) / 2./M_PI * (Y_ENC_STEPSPERREV)))
// convert angle in radians to +-pi // convert angle in radians to +-pi
static inline double ang2half(double ang){ static inline __attribute__((always_inline)) double ang2half(double ang){
if(ang < -M_PI) ang += 2.*M_PI; if(ang < -M_PI) ang += 2.*M_PI;
else if(ang > M_PI) ang -= 2.*M_PI; else if(ang > M_PI) ang -= 2.*M_PI;
return ang; return ang;
} }
// convert to only positive: 0..2pi // convert to only positive: 0..2pi
static inline double ang2full(double ang){ static inline __attribute__((always_inline)) double ang2full(double ang){
if(ang < 0.) ang += 2.*M_PI; if(ang < 0.) ang += 2.*M_PI;
else if(ang > 2.*M_PI) ang -= 2.*M_PI; else if(ang > 2.*M_PI) ang -= 2.*M_PI;
return ang; return ang;
} }
// motor position to radians and back // motor position to radians and back
#define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV) #define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (X_MOT_STEPSPERREV))
#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV) #define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / (Y_MOT_STEPSPERREV))
#define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * X_MOT_STEPSPERREV)) #define X_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (X_MOT_STEPSPERREV)))
#define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV)) #define Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * (Y_MOT_STEPSPERREV)))
// motor speed in rad/s and back // motor speed in rad/s and back
#define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY) #define X_MOTSPD2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY) #define Y_MOTSPD2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY))
#define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY)) #define X_RS2MOTSPD(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY)) #define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY)))
// motor acceleration -//- // motor acceleration -//-
#define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY) #define X_MOTACC2RS(n) (X_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
#define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * SITECH_LOOP_FREQUENCY * SITECH_LOOP_FREQUENCY) #define Y_MOTACC2RS(n) (Y_MOT2RAD(n) / 65536. * (SITECH_LOOP_FREQUENCY) * (SITECH_LOOP_FREQUENCY))
#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY)) #define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / SITECH_LOOP_FREQUENCY / SITECH_LOOP_FREQUENCY)) #define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r) * 65536. / (SITECH_LOOP_FREQUENCY) / (SITECH_LOOP_FREQUENCY)))
// adder time to seconds vice versa // adder time to seconds vice versa
#define ADDER2S(a) ((a) / SITECH_LOOP_FREQUENCY) #define ADDER2S(a) ((a) / (SITECH_LOOP_FREQUENCY))
#define S2ADDER(s) ((s) * SITECH_LOOP_FREQUENCY) #define S2ADDER(s) ((s) * (SITECH_LOOP_FREQUENCY))
// encoder's tolerance (ticks) // encoder's tolerance (ticks)
#define YencTOL (25.) #define YencTOL (25.)