diff --git a/LibSidServo/examples/CMakeLists.txt b/LibSidServo/examples/CMakeLists.txt index a4085b9..3851785 100644 --- a/LibSidServo/examples/CMakeLists.txt +++ b/LibSidServo/examples/CMakeLists.txt @@ -5,4 +5,5 @@ include_directories(../) link_libraries(sidservo usefull_macros) # exe list -add_executable(dumpmoving dumpmoving.c) +add_executable(dump dumpmoving.c dump.c) +add_executable(dump_s dumpmoving_scmd.c dump.c) diff --git a/LibSidServo/examples/dump.c b/LibSidServo/examples/dump.c new file mode 100644 index 0000000..f5746af --- /dev/null +++ b/LibSidServo/examples/dump.c @@ -0,0 +1,106 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// logging of mount position + +#include + +#include "dump.h" +#include "simpleconv.h" + +/** + * @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 || !m) return; + 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); +} + +/** + * @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 millis = mdata.millis; + 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){ + usleep(10000); + if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} + if(mdata.millis == millis) continue; + DBG("Got new data, posX=%g, posY=%g", mdata.motposition.X, mdata.motposition.Y); + millis = mdata.millis; + if(fcoords) logmnt(fcoords, &mdata); + if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ + xlast = mdata.motposition.X; + ylast = mdata.motposition.Y; + ctr = 0; + }else ++ctr; + } +} + +/** + * @brief waitmoving - wait until moving by both axes 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.; + while(ctr < N){ + usleep(10000); + if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;} + if(mdata.millis == millis) continue; + if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){ + xlast = mdata.motposition.X; + ylast = mdata.motposition.Y; + ctr = 0; + }else ++ctr; + } +} diff --git a/LibSidServo/examples/dump.h b/LibSidServo/examples/dump.h new file mode 100644 index 0000000..7755bdf --- /dev/null +++ b/LibSidServo/examples/dump.h @@ -0,0 +1,27 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once + +#include + +#include "sidservo.h" + +void logmnt(FILE *fcoords, mountdata_t *m); +void dumpmoving(FILE *fcoords, double t, int N); +void waitmoving(int N); diff --git a/LibSidServo/examples/dumpmoving.c b/LibSidServo/examples/dumpmoving.c index 6759238..e08b80e 100644 --- a/LibSidServo/examples/dumpmoving.c +++ b/LibSidServo/examples/dumpmoving.c @@ -16,28 +16,36 @@ * along with this program. If not, see . */ +// dump telescope moving using simplest goto command + #include #include #include #include #include +#include "dump.h" #include "sidservo.h" +#include "simpleconv.h" typedef struct{ int help; int verbose; + int Ncycles; char *logfile; char *coordsoutput; } parameters; -static parameters G = {0}; +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"}, end_option }; @@ -61,59 +69,6 @@ static conf_t Config = { .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); @@ -124,8 +79,8 @@ int main(int argc, char **argv){ if(G.coordsoutput){ if(!(fcoords = fopen(G.coordsoutput, "w"))) ERRX("Can't open %s", G.coordsoutput); - logmnt(NULL); - } + }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); @@ -142,9 +97,9 @@ int main(int argc, char **argv){ signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.))) ERRX("Can't move to 45, 45"); - dumpmoving(30.); + dumpmoving(fcoords, 30., G.Ncycles); Mount.moveTo(0., 0.); - dumpmoving(30.); + dumpmoving(fcoords, 30., G.Ncycles); signals(0); return 0; } diff --git a/LibSidServo/examples/dumpmoving_scmd.c b/LibSidServo/examples/dumpmoving_scmd.c new file mode 100644 index 0000000..877e613 --- /dev/null +++ b/LibSidServo/examples/dumpmoving_scmd.c @@ -0,0 +1,149 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// dump telescope moving using short binary commands + +#include +#include +#include +#include +#include +#include + +#include "dump.h" +#include "sidservo.h" +#include "simpleconv.h" +#include "ssii.h" + +typedef struct{ + int help; + int Ncycles; + char *coordsoutput; +} 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"}, + {"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"}, + end_option +}; + +void signals(int sig){ + if(sig){ + signal(sig, SIG_IGN); + DBG("Get signal %d, quit.\n", sig); + } + Mount.quit(); + exit(sig); +} + +static conf_t Config = { + .MountDevPath = "/dev/ttyUSB0", + .MountDevSpeed = 19200, + //.EncoderDevPath = "/dev/ttyUSB1", + //.EncoderDevSpeed = 153000, + .MountReqInterval = 0.1, + .SepEncoder = 0 +}; + +// dump thread +static void *dumping(void _U_ *u){ + dumpmoving(fcoords, 90., G.Ncycles); + return NULL; +} + +// return TRUE if motor position is reached +-2 counts +#define X2count (X_RAD2MOT(2)) +#define Y2count (Y_RAD2MOT(2)) +static int Xreached(double xtag){ + mountdata_t mdata; + if(MCC_E_OK != Mount.getMountData(&mdata)) return FALSE; + if(fabs(mdata.motposition.X - xtag) <= X2count) return TRUE; + return FALSE; +} +/* +static int Yreached(double ytag){ + mountdata_t mdata; + if(MCC_E_OK != Mount.getMountData(&mdata)) return FALSE; + if(fabs(mdata.motposition.Y - ytag) <= Y2count) return TRUE; + return FALSE; +} +*/ + +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; + logmnt(fcoords, NULL); + 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 + // move to X=30 degr with different speeds + // start with 1degr/s, increase to 15, decrease to 1 + short_command_t cmd = {0}; + pthread_t dthr; + if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); +#define SCMD() do{if(MCC_E_OK != Mount.shortCmd(&cmd)) ERRX("Can't run command"); }while(0) + // goto 5 degr with 1deg/s + cmd.Xmot = DEG2RAD(30.); + cmd.Xspeed = DEG2RAD(1.); + SCMD(); + while(!Xreached(DEG2RAD(5.))); + // goto 15 degr with 5deg/s + cmd.Xmot = DEG2RAD(30.); + cmd.Xspeed = DEG2RAD(5.); + SCMD(); + while(!Xreached(DEG2RAD(10.))); + // goto 25 degr with 15deg/s + cmd.Xmot = DEG2RAD(30.); + cmd.Xspeed = DEG2RAD(15.); + SCMD(); + while(!Xreached(DEG2RAD(25.))); + // goto 29.9 degr with 1deg/s + cmd.Xmot = DEG2RAD(30.); + cmd.Xspeed = DEG2RAD(1.); + SCMD(); + while(!Xreached(DEG2RAD(29.9))); + // and go back with 15deg/s + cmd.Xmot = DEG2RAD(0.); + cmd.Xspeed = DEG2RAD(15.); + SCMD(); + while(!Xreached(DEG2RAD(0.1))); + Mount.moveTo(0., 0.); + // wait moving ends + pthread_join(dthr, NULL); +#undef SCMD + signals(0); + return 0; +} diff --git a/LibSidServo/examples/simpleconv.h b/LibSidServo/examples/simpleconv.h new file mode 100644 index 0000000..284b539 --- /dev/null +++ b/LibSidServo/examples/simpleconv.h @@ -0,0 +1,24 @@ +/* + * This file is part of the libsidservo project. + * Copyright 2025 Edward V. Emelianov . + * + * 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 . + */ + +// simple conversion macros + +#include + +#define DEG2RAD(d) (d/180.*M_PI) +#define RAD2DEG(d) (d/M_PI*180.) diff --git a/LibSidServo/libsidservo.creator.user b/LibSidServo/libsidservo.creator.user index 6016c1c..fa4a797 100644 --- a/LibSidServo/libsidservo.creator.user +++ b/LibSidServo/libsidservo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/LibSidServo/libsidservo.files b/LibSidServo/libsidservo.files index e6551f6..2fa5f99 100644 --- a/LibSidServo/libsidservo.files +++ b/LibSidServo/libsidservo.files @@ -1,6 +1,10 @@ CMakeLists.txt dbg.h +examples/dump.c +examples/dump.h examples/dumpmoving.c +examples/dumpmoving_scmd.c +examples/simpleconv.h main.c sidservo.h serial.c diff --git a/LibSidServo/main.c b/LibSidServo/main.c index 450efb7..5093b7f 100644 --- a/LibSidServo/main.c +++ b/LibSidServo/main.c @@ -84,11 +84,69 @@ static mcc_errcodes_t move2(double X, double Y){ return MCC_E_OK; } +/** + * @brief emstop - emergency stop + * @return errcode + */ static mcc_errcodes_t emstop(){ if(!SSemergStop()) return MCC_E_FAILED; return MCC_E_OK; } +/** + * @brief shortcmd - send and receive short binary command + * @param cmd (io) - command + * @return errcode + */ +static mcc_errcodes_t shortcmd(short_command_t *cmd){ + if(!cmd) return MCC_E_BADFORMAT; + SSscmd s = {0}; + s.Xmot = X_RAD2MOT(cmd->Xmot); + s.Ymot = Y_RAD2MOT(cmd->Ymot); + s.Xspeed = X_RS2MOTSPD(cmd->Xspeed); + s.Yspeed = Y_RS2MOTSPD(cmd->Yspeed); + s.xychange = cmd->xychange; + s.XBits = cmd->XBits; + s.YBits = cmd->YBits; + if(SScmdS(&s)) return MCC_E_FAILED; + cmd->Xmot = X_MOT2RAD(s.Xmot); + cmd->Ymot = Y_MOT2RAD(s.Ymot); + cmd->Xspeed = X_MOTSPD2RS(s.Xspeed); + cmd->Yspeed = Y_MOTSPD2RS(s.Yspeed); + cmd->xychange = s.xychange; + cmd->XBits = s.XBits; + cmd->YBits = s.YBits; + return MCC_E_OK; +} + +/** + * @brief shortcmd - send and receive long binary command + * @param cmd (io) - command + * @return errcode + */ +static mcc_errcodes_t longcmd(long_command_t *cmd){ + if(!cmd) return MCC_E_BADFORMAT; + SSlcmd l = {0}; + l.Xmot = X_RAD2MOT(cmd->Xmot); + l.Ymot = Y_RAD2MOT(cmd->Ymot); + l.Xspeed = X_RS2MOTSPD(cmd->Xspeed); + l.Yspeed = Y_RS2MOTSPD(cmd->Yspeed); + l.Xadder = X_RS2MOTSPD(cmd->Xadder); + l.Yadder = Y_RS2MOTSPD(cmd->Yadder); + l.Xatime = S2ADDER(cmd->Xatime); + l.Yatime = S2ADDER(cmd->Yatime); + if(SScmdL(&l)) return MCC_E_FAILED; + cmd->Xmot = X_MOT2RAD(l.Xmot); + cmd->Ymot = Y_MOT2RAD(l.Ymot); + cmd->Xspeed = X_MOTSPD2RS(l.Xspeed); + cmd->Yspeed = Y_MOTSPD2RS(l.Yspeed); + cmd->Xadder = X_MOTSPD2RS(l.Xadder); + cmd->Yadder = Y_MOTSPD2RS(l.Yadder); + cmd->Xatime = ADDER2S(l.Xatime); + cmd->Yatime = ADDER2S(l.Yatime); + return MCC_E_OK; +} + // init mount class mount_t Mount = { .init = init, @@ -96,4 +154,6 @@ mount_t Mount = { .getMountData = getMD, .moveTo = move2, .emergStop = emstop, + .shortCmd = shortcmd, + .longCmd = longcmd, }; diff --git a/LibSidServo/serial.c b/LibSidServo/serial.c index c1fa9ac..ecab794 100644 --- a/LibSidServo/serial.c +++ b/LibSidServo/serial.c @@ -227,7 +227,8 @@ static void *mountthread(void _U_ *u){ // data to get data_t d = {.buf = buf, .maxlen = sizeof(buf)}; // cmd to send - const data_t *cmd_getstat = cmd2dat(CMD_GETSTAT); + data_t *cmd_getstat = cmd2dat(CMD_GETSTAT); + if(!cmd_getstat) goto failed; double t0 = dtime(); /* #ifdef EBUG @@ -256,6 +257,8 @@ static void *mountthread(void _U_ *u){ while(dtime() - t0 < Conf.MountReqInterval); t0 = dtime(); } + data_free(&cmd_getstat); +failed: if(mntfd > -1){ close(mntfd); mntfd = -1; diff --git a/LibSidServo/sidservo.h b/LibSidServo/sidservo.h index 63d4a6d..a47df75 100644 --- a/LibSidServo/sidservo.h +++ b/LibSidServo/sidservo.h @@ -73,6 +73,27 @@ typedef struct{ double voltage; } mountdata_t; +typedef struct{ + int32_t Xmot; // 0 X motor position (rad) + int32_t Xspeed; // 4 X speed (rad/s) + int32_t Ymot; // 8 + int32_t Yspeed; // 12 + uint8_t xychange; // 16 change Xbits/Ybits value + uint8_t XBits; // 17 + uint8_t YBits; // 18 +} short_command_t; // short command + +typedef struct{ + int32_t Xmot; // 0 X motor position (rad) + int32_t Xspeed; // 4 X speed (rad/s) + int32_t Ymot; // 8 + int32_t Yspeed; // 12 + int32_t Xadder; // 16 - X adder (rad/s) + int32_t Yadder; // 20 + int32_t Xatime; // 24 X adder time, sec + int32_t Yatime; // 28 +} long_command_t; // long command + // mount class typedef struct{ mcc_errcodes_t (*init)(conf_t *c); // init device @@ -80,6 +101,8 @@ typedef struct{ mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data mcc_errcodes_t (*moveTo)(double X, double Y); // move to given position ans stop mcc_errcodes_t (*emergStop)(); // emergency stop + mcc_errcodes_t (*shortCmd)(short_command_t *cmd); // send/get short command + mcc_errcodes_t (*longCmd)(long_command_t *cmd); // send/get long command } mount_t; extern mount_t Mount; diff --git a/LibSidServo/ssii.c b/LibSidServo/ssii.c index d5372a2..f20d7e1 100644 --- a/LibSidServo/ssii.c +++ b/LibSidServo/ssii.c @@ -174,3 +174,4 @@ int SSemergStop(){ if(i == 10) return FALSE; return TRUE; } + diff --git a/LibSidServo/ssii.h b/LibSidServo/ssii.h index aed0ca5..309bc74 100644 --- a/LibSidServo/ssii.h +++ b/LibSidServo/ssii.h @@ -81,23 +81,29 @@ // exit ASCII checksum mode #define CMD_EXITACM ("YXY0\r\xb8") - -// timeout (seconds) of reading answer (from last symbol read) -#define READTIMEOUT (0.05) - // steps per revolution -#define X_MOT_STEPSPERREV (4394294.) -#define Y_MOT_STEPSPERREV (3325291.) +//#define X_MOT_STEPSPERREV (3325440.) +#define X_MOT_STEPSPERREV (3325952.) +//#define Y_MOT_STEPSPERREV (4394496.) +#define Y_MOT_STEPSPERREV (4394960.) // motor position to radians and back #define X_MOT2RAD(n) (2.*M_PI * (double)n / X_MOT_STEPSPERREV) #define Y_MOT2RAD(n) (2.*M_PI * (double)n / Y_MOT_STEPSPERREV) #define X_RAD2MOT(r) ((uint32_t)(r / 2./M_PI * X_MOT_STEPSPERREV)) #define Y_RAD2MOT(r) ((uint32_t)(r / 2./M_PI * Y_MOT_STEPSPERREV)) +// motor speed in rad/s and back +#define X_MOTSPD2RS(n) (X_MOT2RAD(n)/65536.*1953.) +#define X_RS2MOTSPD(r) ((uint32_t)(X_RAD2MOT(r)*65536./1953.)) +#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n)/65536.*1953.) +#define Y_RS2MOTSPD(r) ((uint32_t)(Y_RAD2MOT(r)*65536./1953.)) +// adder time to seconds vice versa +#define ADDER2S(a) (a*1953.) +#define S2ADDER(s) (s/1953.) // encoder per revolution -#define X_ENC_STEPSPERREV (67108863.) -#define Y_ENC_STEPSPERREV (67108863.) +#define X_ENC_STEPSPERREV (67108864.) +#define Y_ENC_STEPSPERREV (67108864.) // encoder position to radians and back #define X_ENC2RAD(n) (2.*M_PI * (double)n / X_ENC_STEPSPERREV) #define Y_ENC2RAD(n) (2.*M_PI * (double)n / Y_ENC_STEPSPERREV) @@ -131,8 +137,8 @@ typedef struct{ // 41 bytes }__attribute__((packed)) SSstat; typedef struct{ - int32_t Xmot; // 0 DEC motor position - int32_t Xspeed; // 4 DEC speed + int32_t Xmot; // 0 X motor position + int32_t Xspeed; // 4 X speed int32_t Ymot; // 8 int32_t Yspeed; // 12 uint8_t xychange; // 16 change Xbits/Ybits value @@ -142,13 +148,13 @@ typedef struct{ } __attribute__((packed)) SSscmd; // short command typedef struct{ - int32_t Xmot; // 0 DEC motor position - int32_t Xspeed; // 4 DEC speed + int32_t Xmot; // 0 X motor position + int32_t Xspeed; // 4 X speed int32_t Ymot; // 8 int32_t Yspeed; // 12 - int32_t Xadder; // 16 - DEC adder + int32_t Xadder; // 16 - X adder int32_t Yadder; // 20 - int32_t Xatime; // 24 DEC adder time + int32_t Xatime; // 24 X adder time (1953 == 1s) int32_t Yatime; // 28 uint16_t checksum; // 32 } __attribute__((packed)) SSlcmd; // long command @@ -162,3 +168,4 @@ int SSgetint(const char *cmd, int64_t *ans); int SSXmoveto(double pos); int SSYmoveto(double pos); int SSemergStop(); +int SSshortCmd(SSscmd *cmd);