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

shit happens

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-02-25 23:32:51 +03:00
parent f83f95c9cc
commit 5b89a968eb
17 changed files with 214 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
LibSidServo/examples/CMakeLists.txt
View File

@ -5,8 +5,8 @@ include_directories(../)
link_libraries(sidservo usefull_macros -lm)
# exe list
add_executable(goto goto.c dump.c)
add_executable(dump dumpmoving.c dump.c)
add_executable(dump_s dumpmoving_scmd.c dump.c)
add_executable(dumpswing dumpswing.c dump.c)
add_executable(traectory_s scmd_traectory.c dump.c traectories.c)
add_executable(goto goto.c dump.c conf.c)
add_executable(dump dumpmoving.c dump.c conf.c)
add_executable(dump_s dumpmoving_scmd.c dump.c conf.c)
add_executable(dumpswing dumpswing.c dump.c conf.c)
add_executable(traectory_s scmd_traectory.c dump.c traectories.c conf.c)

2
LibSidServo/examples/Readme.md
View File

@ -3,6 +3,8 @@ Some examples of usage of libsidservo
## Auxiliary files
*conf.c*, *conf.h* - base configuration - read from file (default: servo.conf) - to simplify examples running when config changes
*dump.c*, *dump.h* - base logging and dumping functions, also some useful functions like get current position and move to zero if current position isn't at zero.
*traectories.c*, *traectories.h* - modeling simple moving object traectories; also some functions like get current position in encoders' angles setting to zero at motors' zero.

65
LibSidServo/examples/conf.c Normal file
View File

@ -0,0 +1,65 @@
/*
* 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/>.
*/
#include <stdio.h>
#include <usefull_macros.h>
#include "conf.h"
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
.EncoderDevPath = "/dev/ttyUSB1",
.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 1
};
static sl_option_t opts[] = {
{"MountDevPath", NEED_ARG, NULL, 0, arg_string, APTR(&Config.MountDevPath), "path to mount device"},
{"MountDevSpeed", NEED_ARG, NULL, 0, arg_int, APTR(&Config.MountDevSpeed), "serial speed of mount device"},
{"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"},
end_option
};
conf_t *readServoConf(const char *filename){
if(!filename) filename = DEFCONFFILE;
int n = sl_conf_readopts(filename, opts);
if(n < 0){
WARNX("Can't read file %s", filename);
return NULL;
}
if(n == 0){
WARNX("Got ZERO parameters from %s", filename);
return NULL;
}
return &Config;
}
void dumpConf(){
char *c = sl_print_opts(opts, TRUE);
printf("Current configuration:\n%s\n", c);
FREE(c);
}
void confHelp(){
sl_showhelp(-1, opts);
}

27
LibSidServo/examples/conf.h Normal file
View File

@ -0,0 +1,27 @@
/*
* 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/>.
*/
#pragma once
#include "sidservo.h"
#define DEFCONFFILE "servo.conf"
void confHelp();
conf_t *readServoConf(const char *filename);
void dumpConf();

12
LibSidServo/examples/dump.c
View File

@ -36,8 +36,8 @@ void logmnt(FILE *fcoords, mountdata_t *m){
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;
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;
// 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),
@ -64,17 +64,17 @@ void dumpmoving(FILE *fcoords, double t, int N){
WARNX("Can't get mount data");
LOGWARN("Can't get mount data");
}
uint32_t millis = mdata.millis;
uint32_t millis = mdata.encposition.msrtime.tv_usec;
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);
usleep(1000);
if(MCC_E_OK != Mount.getMountData(&mdata)){ WARNX("Can't get data"); continue;}
if(mdata.millis == millis) 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.millis;
millis = mdata.encposition.msrtime.tv_usec;
if(fcoords) logmnt(fcoords, &mdata);
if(mdata.motposition.X != xlast || mdata.motposition.Y != ylast){
xlast = mdata.motposition.X;

35
LibSidServo/examples/dumpmoving.c
View File

@ -24,6 +24,7 @@
#include <time.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
@ -34,6 +35,7 @@ typedef struct{
int Ncycles;
char *logfile;
char *coordsoutput;
char *conffile;
} parameters;
static parameters G = {
@ -47,6 +49,7 @@ static sl_option_t cmdlnopts[] = {
{"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"},
{"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"},
end_option
};
@ -60,15 +63,6 @@ void signals(int sig){
exit(sig);
}
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 0
};
int main(int argc, char **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
@ -76,6 +70,11 @@ 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);
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();
return 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
@ -83,23 +82,21 @@ int main(int argc, char **argv){
logmnt(fcoords, NULL);
time_t curtime = time(NULL);
LOGMSG("Started @ %s", ctime(&curtime));
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;
}
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;
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 tag = DEG2RAD(45.), zero = 0.;
if(MCC_E_OK != Mount.moveTo(&tag, &tag))
double tagx = DEG2RAD(45.) + M.X, tagy = DEG2RAD(45.) + M.Y;
if(MCC_E_OK != Mount.moveTo(&tagx, &tagy))
ERRX("Can't move to 45, 45");
dumpmoving(fcoords, 30., G.Ncycles);
Mount.moveTo(&zero, &zero);
Mount.moveTo(&M.X, &M.Y);
dumpmoving(fcoords, 30., G.Ncycles);
signals(0);
return 0;

41
LibSidServo/examples/dumpmoving_scmd.c
View File

@ -26,6 +26,7 @@
#include <time.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
@ -33,24 +34,31 @@
typedef struct{
int help;
int Ncycles;
int relative;
double reqint;
char *coordsoutput;
char *conffile;
char *axis;
} parameters;
static parameters G = {
.Ncycles = 40,
.reqint = 0.1,
.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 or Y)"},
{"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_int, APTR(&G.conffile), "configuration file name"},
{"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"},
end_option
};
@ -63,17 +71,6 @@ void signals(int sig){
exit(sig);
}
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 0
};
static coords_t M;
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
@ -115,11 +112,12 @@ 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'){
if(*G.axis == 'X' || *G.axis == 'B'){
cmd.Xmot = DEG2RAD(target) + M.X;
cmd.Xspeed = DEG2RAD(speed);
limit = DEG2RAD(limit) + M.X;
}else{
}
if(*G.axis == 'Y' || *G.axis == 'B'){
cmd.Ymot = DEG2RAD(target) + M.Y;
cmd.Yspeed = DEG2RAD(speed);
limit = DEG2RAD(limit) + M.Y;
@ -134,16 +132,21 @@ 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")){
WARNX("\"Axis\" should be X or Y");
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;
Config.MountReqInterval = G.reqint;
if(MCC_E_OK != Mount.init(&Config)){
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;
}

19
LibSidServo/examples/dumpswing.c
View File

@ -22,6 +22,7 @@
#include <string.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
@ -35,6 +36,7 @@ typedef struct{
double period;
double amplitude;
char *coordsoutput;
char *conffile;
char *axis;
} parameters;
@ -55,6 +57,7 @@ static sl_option_t cmdlnopts[] = {
{"period", NEED_ARG, NULL, 'p', arg_double, APTR(&G.period), "swinging period (could be not reached if amplitude is too small) - not more than 900s (default: 1)"},
{"amplitude", NEED_ARG, NULL, 'A', arg_double, APTR(&G.amplitude), "max amplitude (could be not reaced if period is too small) - not more than 45deg (default: 5)"},
{"nswings", NEED_ARG, NULL, 'N', arg_int, APTR(&G.Nswings), "amount of swing periods (default: 10)"},
{"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"},
end_option
};
@ -67,15 +70,6 @@ void signals(int sig){
exit(sig);
}
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.05,
.SepEncoder = 0
};
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
@ -125,7 +119,12 @@ int main(int argc, char **argv){
if(G.period < 0.1 || G.period > 900.)
ERRX("Period should be from 0.1 to 900s");
if(G.Nswings < 1) ERRX("Nswings should be more than 0");
mcc_errcodes_t e = Mount.init(&Config);
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();
return 1;
}
mcc_errcodes_t e = Mount.init(Config);
if(e != MCC_E_OK){
WARNX("Can't init devices");
return 1;

27
LibSidServo/examples/goto.c
View File

@ -23,6 +23,7 @@
#include <time.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
@ -33,6 +34,7 @@ typedef struct{
int wait;
int relative;
char *coordsoutput;
char *conffile;
double X;
double Y;
} parameters;
@ -51,18 +53,10 @@ static sl_option_t cmdlnopts[] = {
{"output", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"file to log coordinates"},
{"wait", NO_ARGS, NULL, 'w', arg_int, APTR(&G.wait), "wait until mowing stopped"},
{"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"},
{"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"},
end_option
};
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.1,
.SepEncoder = 0
};
static FILE* fcoords = NULL;
static pthread_t dthr;
@ -86,8 +80,14 @@ static void *dumping(void _U_ *u){
int main(int _U_ argc, char _U_ **argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
if(G.help) sl_showhelp(-1, cmdlnopts);
if(MCC_E_OK != Mount.init(&Config)) ERRX("Can't init mount");
if(G.help)
sl_showhelp(-1, cmdlnopts);
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();
return 1;
}
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
coords_t M;
if(!getPos(&M, NULL)) ERRX("Can't get current position");
if(G.coordsoutput){
@ -126,6 +126,9 @@ int main(int _U_ argc, char _U_ **argv){
}
out:
if(G.coordsoutput) pthread_join(dthr, NULL);
if(G.wait) Mount.quit();
if(G.wait){
if(getPos(&M, NULL)) printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X), RAD2DEG(M.Y));
Mount.quit();
}
return 0;
}

21
LibSidServo/examples/scmd_traectory.c
View File

@ -20,6 +20,7 @@
#include <signal.h>
#include <usefull_macros.h>
#include "conf.h"
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
@ -38,6 +39,7 @@ typedef struct{
double Y0; // -//-
char *coordsoutput; // dump file
char *tfn; // traectory function name
char *conffile;
} parameters;
static FILE *fcoords = NULL;
@ -64,18 +66,10 @@ static sl_option_t cmdlnopts[] = {
{"tmax", NEED_ARG, NULL, 'T', arg_double, APTR(&G.tmax), "maximal duration time of emulation (default: 300 seconds)"},
{"x0", NEED_ARG, NULL, '0', arg_double, APTR(&G.X0), "starting X-coordinate of traectory (default: 10 degrees)"},
{"y0", NEED_ARG, NULL, '1', arg_double, APTR(&G.Y0), "starting Y-coordinate of traectory (default: 10 degrees)"},
{"conffile", NEED_ARG, NULL, 'C', arg_int, APTR(&G.conffile), "configuration file name"},
end_option
};
static conf_t Config = {
.MountDevPath = "/dev/ttyUSB0",
.MountDevSpeed = 19200,
//.EncoderDevPath = "/dev/ttyUSB1",
//.EncoderDevSpeed = 153000,
.MountReqInterval = 0.05,
.SepEncoder = 0
};
void signals(int sig){
pthread_cancel(dthr);
if(sig){
@ -127,7 +121,12 @@ int main(int argc, char **argv){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
Config.MountReqInterval = G.reqint;
conf_t *Config = readServoConf(G.conffile);
if(!Config){
dumpConf();
return 1;
}
Config->MountReqInterval = G.reqint;
traectory_fn tfn = traectory_by_name(G.tfn);
if(!tfn){
WARNX("Bad traectory name %s, should be one of", G.tfn);
@ -139,7 +138,7 @@ int main(int argc, char **argv){
ERRX("Can't init traectory");
return 1;
}
mcc_errcodes_t e = Mount.init(&Config);
mcc_errcodes_t e = Mount.init(Config);
if(e != MCC_E_OK){
WARNX("Can't init devices");
return 1;

2
LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 15.0.1, 2025-02-24T22:23:07. -->
<!-- Written by QtCreator 15.0.1, 2025-02-25T23:31:43. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

2
LibSidServo/libsidservo.files
View File

@ -1,5 +1,7 @@
CMakeLists.txt
dbg.h
examples/conf.c
examples/conf.h
examples/dump.c
examples/dump.h
examples/dumpmoving.c

6
LibSidServo/main.c
View File

@ -104,12 +104,14 @@ static mcc_errcodes_t move2(const double *X, const double *Y){
if(!X && !Y) return MCC_E_BADFORMAT;
if(X){
if(!chkX(*X)) return MCC_E_BADFORMAT;
int64_t tag = X_RAD2MOT(*X);
int32_t tag = X_RAD2MOT(*X);
DBG("X: %g, tag: %d", *X, tag);
if(!SSsetterI(CMD_MOTX, tag)) return MCC_E_FAILED;
}
if(Y){
if(!chkY(*Y)) return MCC_E_BADFORMAT;
int64_t tag = X_RAD2MOT(*Y);
int32_t tag = Y_RAD2MOT(*Y);
DBG("Y: %g, tag: %d", *Y, tag);
if(!SSsetterI(CMD_MOTY, tag)) return MCC_E_FAILED;
}
return MCC_E_OK;

19
LibSidServo/serial.c
View File

@ -48,8 +48,8 @@ static struct timeval encRtmout = {0}, mntRtmout = {0};
// encoders raw data
typedef struct __attribute__((packed)){
uint8_t magick;
int32_t encX;
int32_t encY;
int32_t encX;
uint8_t CRC[4];
} enc_t;
@ -82,6 +82,13 @@ static void gttime(){
*/
static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
enc_t *edata = (enc_t*) databuf;
/*
#ifdef EBUG
DBG("ENCBUF:");
for(int i = 0; i < ENC_DATALEN; ++i) printf("%02X ", databuf[i]);
printf("\n");
#endif
*/
if(edata->magick != ENC_MAGICK){
DBG("No magick");
return;
@ -112,7 +119,7 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timeval *tv){
mountdata.encposition.Y = Y_ENC2RAD(edata->encY);
mountdata.encposition.msrtime = *tv;
pthread_mutex_unlock(&datamutex);
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);
//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);
}
// try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
@ -181,10 +188,10 @@ static void *encoderthread(void _U_ *u){
if(b == -2) ++errctr;
if(b < 0) continue;
errctr = 0;
DBG("Got byte from Encoder: 0x%02X", b);
// DBG("Got byte from Encoder: 0x%02X", b);
if(wridx == 0){
if((uint8_t)b == ENC_MAGICK){
DBG("Got magic -> start filling packet");
// DBG("Got magic -> start filling packet");
databuf[wridx++] = (uint8_t) b;
gettimeofday(&tv, NULL);
}
@ -239,7 +246,11 @@ static void *mountthread(void _U_ *u){
struct timeval tgot;
if(0 != gettimeofday(&tgot, NULL)) continue;
if(!MountWriteRead(cmd_getstat, &d) || d.len != sizeof(SSstat)){
#ifdef EBUG
DBG("Can't read SSstat, need %zd got %zd bytes", sizeof(SSstat), d.len);
for(size_t i = 0; i < d.len; ++i) printf("%02X ", d.buf[i]);
printf("\n");
#endif
++errctr; continue;
}
if(SScalcChecksum(buf, sizeof(SSstat)-2) != status->checksum){

4
LibSidServo/sidservo.h
View File

@ -98,6 +98,10 @@ typedef struct{
uint32_t millis;
double temperature;
double voltage;
int32_t XmotRaw;
int32_t YmotRaw;
int32_t XencRaw;
int32_t YencRaw;
} mountdata_t;
typedef struct{

2
LibSidServo/ssii.c
View File

@ -137,7 +137,7 @@ int SSgetint(const char *cmd, int64_t *ans){
*/
int SSsetterI(const char *cmd, int32_t ival){
char buf[128];
snprintf(buf, 127, "%s%" PRIi64, cmd, ival);
snprintf(buf, 127, "%s%" PRIi32, cmd, ival);
return SStextcmd(buf, NULL);
}

22
LibSidServo/ssii.h
View File

@ -168,9 +168,7 @@
// steps per revolution
//#define X_MOT_STEPSPERREV (3325440.)
#define X_MOT_STEPSPERREV (3325952.)
//#define Y_MOT_STEPSPERREV (4394496.)
#define Y_MOT_STEPSPERREV (4394960.)
// maximal speeds in rad/s: 10deg/s by X and 8deg/s by Y
@ -178,27 +176,27 @@
#define Y_SPEED_MAX (0.13963)
// 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) ((int32_t)(r / 2./M_PI * X_MOT_STEPSPERREV))
#define Y_RAD2MOT(r) ((int32_t)(r / 2./M_PI * Y_MOT_STEPSPERREV))
#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) ((int32_t)((r) / 2./M_PI * X_MOT_STEPSPERREV))
#define Y_RAD2MOT(r) ((int32_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) ((int32_t)(X_RAD2MOT(r)*65536./1953.))
#define Y_MOTSPD2RS(n) (Y_MOT2RAD(n)/65536.*1953.)
#define Y_RS2MOTSPD(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953.))
// adder time to seconds vice versa
#define ADDER2S(a) (a*1953.)
#define S2ADDER(s) (s/1953.)
#define ADDER2S(a) ((a)*1953.)
#define S2ADDER(s) ((s)/1953.)
// encoder per revolution
#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)
#define X_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * X_ENC_STEPSPERREV))
#define Y_RAD2ENC(r) ((uint32_t)(r / 2./M_PI * Y_ENC_STEPSPERREV))
#define X_ENC2RAD(n) (2.*M_PI * ((double)n) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) (2.*M_PI * ((double)n) / Y_ENC_STEPSPERREV)
#define X_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * X_ENC_STEPSPERREV))
#define Y_RAD2ENC(r) ((uint32_t)((r) / 2./M_PI * Y_ENC_STEPSPERREV))
// encoder's tolerance (ticks)
#define YencTOL (25.)