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

add dumpswing, fix bug with uint32 instead of int32

Browse Source
This commit is contained in:
Edward V. Emelianov 2025-02-19 23:07:26 +03:00
parent 1195393fa8
commit 357a0d7e19
10 changed files with 207 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
LibSidServo/examples/CMakeLists.txt
View File

@ -2,10 +2,11 @@ project(examples)
# common includes & library
include_directories(../)
link_libraries(sidservo usefull_macros)
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)

1
LibSidServo/examples/dump.c
View File

@ -97,6 +97,7 @@ void waitmoving(int N){
usleep(10000);
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;

172
LibSidServo/examples/dumpswing.c Normal file
View File

@ -0,0 +1,172 @@
/*
* 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 <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <usefull_macros.h>
#include "dump.h"
#include "sidservo.h"
#include "simpleconv.h"
// swing telescope by given axis with given period and max amplitude, reqinterval=0.05 (min)
typedef struct{
int help;
int Ncycles;
int Nswings;
double period;
double amplitude;
char *coordsoutput;
char *axis;
} parameters;
static parameters G = {
.Ncycles = 20,
.axis = "X",
.Nswings = 10,
.period = 1.,
.amplitude = 5.,
};
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: 20)"},
{"coordsfile", NEED_ARG, NULL, 'o', arg_string, APTR(&G.coordsoutput),"output file with coordinates log"},
{"axis", NEED_ARG, NULL, 'a', arg_string, APTR(&G.axis), "axis to move (X or Y)"},
{"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)"},
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.05,
.SepEncoder = 0
};
// dump thread
static void *dumping(void _U_ *u){
dumpmoving(fcoords, 3600., G.Ncycles);
return NULL;
}
// wait until mount is stopped within 5 cycles or until time reached t
void waithalf(double t){
mountdata_t mdata;
int ctr = -1;
uint32_t millis = 0;
double xlast = 0., ylast = 0.;
while(ctr < 5){
if(sl_dtime() >= 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;
ctr = 0;
}else{
DBG("EQ: old=%g, now=%g", RAD2DEG(ylast), RAD2DEG(mdata.motposition.Y));
++ctr;
}
}
}
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");
return 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
if(G.Ncycles < 7) ERRX("Ncycles should be >7");
if(G.amplitude < 0.01 || G.amplitude > 45.)
ERRX("Amplitude should be from 0.01 to 45 degrees");
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);
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
pthread_t dthr;
chk0(G.Ncycles);
logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
G.period /= 2.; // pause between commands
double tagX, tagY;
if(*G.axis == 'X'){
tagX = DEG2RAD(G.amplitude); tagY = 0.;
}else{
tagX = 0.; tagY = DEG2RAD(G.amplitude);
}
double t = sl_dtime(), t0 = t;
double divide = 2.;
for(int i = 0; i < G.Nswings; ++i){
Mount.moveTo(tagX, tagY);
DBG("CMD: %g", sl_dtime()-t0);
t += G.period / divide;
divide = 1.;
waithalf(t);
DBG("Moved to +, t=%g", t-t0);
DBG("CMD: %g", sl_dtime()-t0);
Mount.moveTo(-tagX, -tagY);
t += G.period;
waithalf(t);
DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", sl_dtime()-t0);
}
// be sure to move @ 0,0
Mount.moveTo(0., 0.);
// wait moving ends
pthread_join(dthr, NULL);
#undef SCMD
signals(0);
return 0;
}

18
LibSidServo/examples/goto.c
View File

@ -30,6 +30,7 @@
typedef struct{
int help;
int Ncycles;
int wait;
char *coordsoutput;
double X;
double Y;
@ -47,6 +48,7 @@ static sl_option_t cmdlnopts[] = {
{"newx", NEED_ARG, NULL, 'X', arg_double, APTR(&G.X), "new X coordinate"},
{"newy", NEED_ARG, NULL, 'Y', arg_double, APTR(&G.Y), "new Y coordinate"},
{"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"},
end_option
};
@ -86,6 +88,10 @@ int main(int _U_ argc, char _U_ **argv){
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){
if(!G.wait) green("When logging I should wait until moving ends; added '-w'");
G.wait = 1;
}
if(G.coordsoutput){
if(!(fcoords = fopen(G.coordsoutput, "w")))
ERRX("Can't open %s", G.coordsoutput);
@ -98,12 +104,14 @@ int main(int _U_ argc, char _U_ **argv){
if(isnan(G.Y)) G.Y = RAD2DEG(M.Y);
printf("Moving to X=%g deg, Y=%g deg\n", G.X, G.Y);
Mount.moveTo(DEG2RAD(G.X), DEG2RAD(G.Y));
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));
if(G.wait){
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));
}
out:
if(G.coordsoutput) pthread_join(dthr, NULL);
Mount.quit();
if(G.wait) Mount.quit();
return 0;
}

4
LibSidServo/examples/scmd_traectory.c
View File

@ -93,7 +93,7 @@ static void *dumping(void _U_ *u){
}
// calculate
static void runtraectory(traectory_fn *tfn){
static void runtraectory(traectory_fn tfn){
if(!tfn) return;
coords_t telXY, traectXY;
double t0 = sl_dtime();
@ -128,7 +128,7 @@ int main(int argc, char **argv){
ERRX("Can't open %s", G.coordsoutput);
}else fcoords = stdout;
Config.MountReqInterval = G.reqint;
traectory_fn *tfn = traectory_by_name(G.tfn);
traectory_fn tfn = traectory_by_name(G.tfn);
if(!tfn){
WARNX("Bad traectory name %s, should be one of", G.tfn);
print_tr_names();

15
LibSidServo/examples/traectories.c
View File

@ -26,7 +26,7 @@
#include "simpleconv.h"
#include "traectories.h"
static traectory_fn *cur_traectory = NULL;
static traectory_fn cur_traectory = NULL;
// starting point of traectory
static coords_t XYstart = {0};
static double tstart = 0.;
@ -41,11 +41,11 @@ 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, coords_t *XY0){
if(!f || !XY0) return FALSE;
cur_traectory = f;
XYstart = *XY0;
tstart = cl_dtime();
tstart = sl_dtime();
mountdata_t mdata;
int ntries = 0;
for(; ntries < 10; ++ntries){
@ -71,7 +71,7 @@ int traectory_point(coords_t *nextpt, double t){
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_PI2 || pt.X > M_PI2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE;
if(pt.X < -M_PI_2 || pt.X > M_PI_2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE;
return TRUE;
}
@ -111,7 +111,7 @@ int SinCos(coords_t *nextpt, double t){
}
typedef struct{
traectory_fn *f;
traectory_fn f;
const char *name;
const char *help;
} tr_names;
@ -122,8 +122,8 @@ static tr_names names[] = {
{NULL, NULL, NULL}
};
traectory_fn *traectory_by_name(const char *name){
traectory_fn *f = NULL;
traectory_fn traectory_by_name(const char *name){
traectory_fn f = NULL;
for(int i = 0; ; ++i){
if(!names[i].f) break;
if(strcmp(names[i].name, name) == 0){
@ -131,6 +131,7 @@ traectory_fn *traectory_by_name(const char *name){
break;
}
}
return f;
}
// print all acceptable traectories names with help

6
LibSidServo/examples/traectories.h
View File

@ -23,10 +23,10 @@
// traectory
typedef int (*traectory_fn)(coords_t *, double);
int init_traectory(traectory_fn *f, coords_t *XY0);
traectory_fn *traectory_by_name(const char *name);
int init_traectory(traectory_fn f, coords_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);
int SinCos(coords_t *nextpt, double t);

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-18T22:15:19. -->
<!-- Written by QtCreator 15.0.1, 2025-02-19T23:05:54. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

1
LibSidServo/libsidservo.files
View File

@ -4,6 +4,7 @@ examples/dump.c
examples/dump.h
examples/dumpmoving.c
examples/dumpmoving_scmd.c
examples/dumpswing.c
examples/goto.c
examples/scmd_traectory.c
examples/simpleconv.h

8
LibSidServo/ssii.h
View File

@ -90,13 +90,13 @@
// 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))
#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) ((uint32_t)(X_RAD2MOT(r)*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) ((uint32_t)(Y_RAD2MOT(r)*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.)