mirror of
https://github.com/eddyem/small_tel.git
synced 2025-12-06 02:35:14 +03:00
start configurator
This commit is contained in:
Edward Emelianov
parent
04c98ed557
commit
e898642af9
@ -5,8 +5,9 @@ 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)
|
||||
add_executable(SSIIconf SSIIconf.c conf.c)
|
||||
|
||||
28
Auxiliary_utils/LibSidServo/examples/Readme.md
Normal file
28
Auxiliary_utils/LibSidServo/examples/Readme.md
Normal file
@ -0,0 +1,28 @@
|
||||
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.
|
||||
|
||||
*simpleconv.h*
|
||||
|
||||
|
||||
## Examples
|
||||
|
||||
*dumpmoving.c* (`dump`) - dump moving relative starting point by simplest text commands "X" and "Y".
|
||||
|
||||
*dumpmoving_scmd.c* (`dump_s`) - moving relative starting point using "short" binary command.
|
||||
|
||||
*dumpswing.c* (`dumpswing`) - shake telescope around starting point by one of axis.
|
||||
|
||||
*goto.c* (`goto`) - get current coordinates or go to given (by simplest "X/Y" commands).
|
||||
|
||||
*scmd_traectory.c* (`traectory_s`) - try to move around given traectory using "short" binary commands.
|
||||
|
||||
*SSIIconf.c* (`SSIIconf`) - read/write hardware configuration of controller
|
||||
|
||||
76
Auxiliary_utils/LibSidServo/examples/SSIIconf.c
Normal file
76
Auxiliary_utils/LibSidServo/examples/SSIIconf.c
Normal file
@ -0,0 +1,76 @@
|
||||
/*
|
||||
* 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"
|
||||
#include "sidservo.h"
|
||||
#include "simpleconv.h"
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
int helpargs;
|
||||
int writeconf;
|
||||
char *conffile;
|
||||
char *hwconffile;
|
||||
} parameters;
|
||||
|
||||
static hardware_configuration_t HW = {0};
|
||||
|
||||
static parameters G = {0};
|
||||
|
||||
static sl_option_t cmdlnopts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"help-opts", NO_ARGS, NULL, 'H', arg_int, APTR(&G.helpargs), "configuration help"},
|
||||
{"serconf", NEED_ARG, NULL, 'C', arg_string, APTR(&G.conffile), "serial configuration file name"},
|
||||
{"hwconf", NEED_ARG, NULL, 'i', arg_string, APTR(&G.hwconffile),"SSII configuration file name"},
|
||||
{"writeconf", NO_ARGS, NULL, 0, arg_int, APTR(&G.writeconf), "write configuration (BE CAREFUL!)"},
|
||||
end_option
|
||||
};
|
||||
|
||||
static sl_option_t confopts[] = {
|
||||
{"Xaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Xconf.accel), "X Default Acceleration, rad/s^2"},
|
||||
{"Yaccel", NEED_ARG, NULL, 0, arg_double, APTR(&HW.Yconf.accel), "Y Default Acceleration, rad/s^2"},
|
||||
end_option
|
||||
};
|
||||
|
||||
int main(int argc, char** argv){
|
||||
sl_init();
|
||||
sl_parseargs(&argc, &argv, cmdlnopts);
|
||||
if(G.help)
|
||||
sl_showhelp(-1, cmdlnopts);
|
||||
if(G.helpargs)
|
||||
sl_showhelp(-1, confopts);
|
||||
conf_t *sconf = readServoConf(G.conffile);
|
||||
if(!sconf){
|
||||
dumpConf();
|
||||
return 1;
|
||||
}
|
||||
if(MCC_E_OK != Mount.init(sconf)) ERRX("Can't init mount");
|
||||
if(MCC_E_OK != Mount.getHWconfig(&HW)) ERRX("Can't read configuration");
|
||||
char *c = sl_print_opts(confopts, TRUE);
|
||||
green("Got configuration:\n");
|
||||
printf("%s\n", c);
|
||||
FREE(c);
|
||||
if(G.hwconffile && G.writeconf){
|
||||
;
|
||||
}
|
||||
Mount.quit();
|
||||
return 0;
|
||||
}
|
||||
65
Auxiliary_utils/LibSidServo/examples/conf.c
Normal file
65
Auxiliary_utils/LibSidServo/examples/conf.c
Normal 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
Auxiliary_utils/LibSidServo/examples/conf.h
Normal file
27
Auxiliary_utils/LibSidServo/examples/conf.h
Normal 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();
|
||||
@ -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;
|
||||
@ -107,13 +107,13 @@ void waitmoving(int N){
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief getMotPos - get current
|
||||
* @brief getPos - get current position
|
||||
* @param mot (o) - motor position (or NULL)
|
||||
* @param Y (o) - encoder position (or NULL)
|
||||
* @return FALSE if failed
|
||||
*/
|
||||
int getPos(coords_t *mot, coords_t *enc){
|
||||
mountdata_t mdata;
|
||||
mountdata_t mdata = {0};
|
||||
int errcnt = 0;
|
||||
do{
|
||||
if(MCC_E_OK != Mount.getMountData(&mdata)) ++errcnt;
|
||||
@ -137,7 +137,8 @@ void chk0(int ncycles){
|
||||
if(!getPos(&M, NULL)) signals(2);
|
||||
if(M.X || M.Y){
|
||||
WARNX("Mount position isn't @ zero; moving");
|
||||
Mount.moveTo(0., 0.);
|
||||
double zero = 0.;
|
||||
Mount.moveTo(&zero, &zero);
|
||||
waitmoving(ncycles);
|
||||
green("Now mount @ zero\n");
|
||||
}
|
||||
|
||||
@ -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_string, 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,22 +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
|
||||
if(MCC_E_OK != Mount.moveTo(DEG2RAD(45.), DEG2RAD(45.)))
|
||||
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(0., 0.);
|
||||
Mount.moveTo(&M.X, &M.Y);
|
||||
dumpmoving(fcoords, 30., G.Ncycles);
|
||||
signals(0);
|
||||
return 0;
|
||||
|
||||
@ -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_string, APTR(&G.conffile), "configuration file name"},
|
||||
{"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"},
|
||||
end_option
|
||||
};
|
||||
|
||||
@ -63,15 +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
|
||||
};
|
||||
|
||||
// dump thread
|
||||
static void *dumping(void _U_ *u){
|
||||
dumpmoving(fcoords, 3600., G.Ncycles);
|
||||
@ -108,20 +107,23 @@ static int Wait(double tag){
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// move X to 40 degr with given speed until given coord
|
||||
// move X/Y to 40 degr with given speed until given coord
|
||||
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'){
|
||||
cmd.Xmot = DEG2RAD(target);
|
||||
if(*G.axis == 'X' || *G.axis == 'B'){
|
||||
cmd.Xmot = DEG2RAD(target) + M.X;
|
||||
cmd.Xspeed = DEG2RAD(speed);
|
||||
}else{
|
||||
cmd.Ymot = DEG2RAD(target);
|
||||
limit = DEG2RAD(limit) + M.X;
|
||||
}
|
||||
if(*G.axis == 'Y' || *G.axis == 'B'){
|
||||
cmd.Ymot = DEG2RAD(target) + M.Y;
|
||||
cmd.Yspeed = DEG2RAD(speed);
|
||||
limit = DEG2RAD(limit) + M.Y;
|
||||
}
|
||||
SCMD();
|
||||
if(!Wait(DEG2RAD(limit))) signals(9);
|
||||
if(!Wait(limit)) signals(9);
|
||||
#undef SCMD
|
||||
}
|
||||
|
||||
@ -130,20 +132,25 @@ 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;
|
||||
mcc_errcodes_t e = Mount.init(&Config);
|
||||
if(e != MCC_E_OK){
|
||||
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;
|
||||
}
|
||||
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
|
||||
@ -151,7 +158,6 @@ int main(int argc, char **argv){
|
||||
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
|
||||
// move to X=40 degr with different speeds
|
||||
pthread_t dthr;
|
||||
chk0(G.Ncycles);
|
||||
logmnt(fcoords, NULL);
|
||||
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
|
||||
// goto 1 degr with 1'/s
|
||||
@ -165,7 +171,7 @@ int main(int argc, char **argv){
|
||||
// and go back with 5deg/s
|
||||
move(0., 0., 5.);
|
||||
// be sure to move @ 0,0
|
||||
Mount.moveTo(0., 0.);
|
||||
Mount.moveTo(&M.X, &M.Y);
|
||||
// wait moving ends
|
||||
pthread_join(dthr, NULL);
|
||||
#undef SCMD
|
||||
|
||||
@ -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;
|
||||
@ -147,23 +146,24 @@ int main(int argc, char **argv){
|
||||
tagX = 0.; tagY = DEG2RAD(G.amplitude);
|
||||
}
|
||||
double t = sl_dtime(), t0 = t;
|
||||
double divide = 2.;
|
||||
double divide = 2., rtagX = -tagX, rtagY = -tagY;
|
||||
for(int i = 0; i < G.Nswings; ++i){
|
||||
Mount.moveTo(tagX, tagY);
|
||||
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);
|
||||
Mount.moveTo(&rtagX, &rtagY);
|
||||
t += G.period;
|
||||
waithalf(t);
|
||||
DBG("Moved to -, t=%g", t-t0);
|
||||
DBG("CMD: %g", sl_dtime()-t0);
|
||||
}
|
||||
double zero = 0.;
|
||||
// be sure to move @ 0,0
|
||||
Mount.moveTo(0., 0.);
|
||||
Mount.moveTo(&zero, &zero);
|
||||
// wait moving ends
|
||||
pthread_join(dthr, NULL);
|
||||
#undef SCMD
|
||||
|
||||
@ -23,6 +23,7 @@
|
||||
#include <time.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "conf.h"
|
||||
#include "dump.h"
|
||||
#include "sidservo.h"
|
||||
#include "simpleconv.h"
|
||||
@ -31,7 +32,9 @@ typedef struct{
|
||||
int help;
|
||||
int Ncycles;
|
||||
int wait;
|
||||
int relative;
|
||||
char *coordsoutput;
|
||||
char *conffile;
|
||||
double X;
|
||||
double Y;
|
||||
} parameters;
|
||||
@ -49,18 +52,11 @@ static sl_option_t cmdlnopts[] = {
|
||||
{"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"},
|
||||
{"relative", NO_ARGS, NULL, 'r', arg_int, APTR(&G.relative), "relative move"},
|
||||
{"conffile", NEED_ARG, NULL, 'C', arg_string, 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;
|
||||
|
||||
@ -84,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){
|
||||
@ -100,10 +102,22 @@ 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;
|
||||
if(isnan(G.X)) G.X = RAD2DEG(M.X);
|
||||
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));
|
||||
double *xtag = NULL, *ytag = NULL, xr, yr;
|
||||
if(!isnan(G.X)){
|
||||
xr = DEG2RAD(G.X);
|
||||
if(G.relative) xr += M.X;
|
||||
xtag = &xr;
|
||||
}
|
||||
if(!isnan(G.Y)){
|
||||
yr = DEG2RAD(G.Y);
|
||||
if(G.relative) yr += M.Y;
|
||||
ytag = &yr;
|
||||
}
|
||||
printf("Moving to ");
|
||||
if(xtag) printf("X=%gdeg ", G.X);
|
||||
if(ytag) printf("Y=%gdeg", G.Y);
|
||||
printf("\n");
|
||||
Mount.moveTo(xtag, ytag);
|
||||
if(G.wait){
|
||||
sleep(1);
|
||||
waitmoving(G.Ncycles);
|
||||
@ -112,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;
|
||||
}
|
||||
|
||||
@ -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_string, 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;
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 15.0.1, 2025-02-19T23:05:54. -->
|
||||
<!-- Written by QtCreator 15.0.1, 2025-02-27T22:45:06. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
|
||||
@ -1,5 +1,8 @@
|
||||
CMakeLists.txt
|
||||
dbg.h
|
||||
examples/SSIIconf.c
|
||||
examples/conf.c
|
||||
examples/conf.h
|
||||
examples/dump.c
|
||||
examples/dump.h
|
||||
examples/dumpmoving.c
|
||||
|
||||
@ -16,6 +16,7 @@
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
@ -30,7 +31,7 @@ conf_t Conf = {0};
|
||||
*/
|
||||
static void quit(){
|
||||
DBG("Close serial devices");
|
||||
for(int i = 0; i < 10; ++i) if(SSemergStop()) break;
|
||||
for(int i = 0; i < 10; ++i) if(SSstop(TRUE)) break;
|
||||
closeSerial();
|
||||
DBG("Exit");
|
||||
}
|
||||
@ -61,26 +62,101 @@ static mcc_errcodes_t init(conf_t *c){
|
||||
ret = MCC_E_ENCODERDEV;
|
||||
}
|
||||
}
|
||||
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.001){
|
||||
if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.05){
|
||||
DBG("Bad value of MountReqInterval");
|
||||
ret = MCC_E_BADFORMAT;
|
||||
}
|
||||
uint8_t buf[1024];
|
||||
data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
|
||||
// read input data as there may be some trash on start
|
||||
if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED;
|
||||
if(ret != MCC_E_OK) quit();
|
||||
return ret;
|
||||
}
|
||||
|
||||
// check coordinates and speeds; return FALSE if failed
|
||||
// TODO fix to real limits!!!
|
||||
static int chkX(double X){
|
||||
if(X > 2.*M_PI || X < -2.*M_PI) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkY(double Y){
|
||||
if(Y > 2.*M_PI || Y < -2.*M_PI) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkXs(double s){
|
||||
if(s < 0. || s > X_SPEED_MAX) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
static int chkYs(double s){
|
||||
if(s < 0. || s > Y_SPEED_MAX) return FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief move2 - simple move to given point and stop
|
||||
* @param X - new X coordinate (radians: -pi..pi)
|
||||
* @param Y - new Y coordinate (radians: -pi..pi)
|
||||
* @param X - new X coordinate (radians: -pi..pi) or NULL
|
||||
* @param Y - new Y coordinate (radians: -pi..pi) or NULL
|
||||
* @return error code
|
||||
*/
|
||||
static mcc_errcodes_t move2(double X, double Y){
|
||||
if(X > M_PI || X < -M_PI || Y > M_PI || Y < -M_PI){
|
||||
DBG("Wrong coords: X=%g, Y=%g", X, Y);
|
||||
return MCC_E_BADFORMAT;
|
||||
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;
|
||||
int32_t tag = X_RAD2MOT(*X);
|
||||
DBG("X: %g, tag: %d", *X, tag);
|
||||
if(!SSsetterI(CMD_MOTX, tag)) return MCC_E_FAILED;
|
||||
}
|
||||
if(!SSXmoveto(X) || !SSYmoveto(Y)) return MCC_E_FAILED;
|
||||
if(Y){
|
||||
if(!chkY(*Y)) return MCC_E_BADFORMAT;
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief setspeed - set maximal speed over axis
|
||||
* @param X (i) - max speed or NULL
|
||||
* @param Y (i) - -//-
|
||||
* @return errcode
|
||||
*/
|
||||
static mcc_errcodes_t setspeed(const double *X, const double *Y){
|
||||
if(!X && !Y) return MCC_E_BADFORMAT;
|
||||
if(X){
|
||||
if(!chkXs(*X)) return MCC_E_BADFORMAT;
|
||||
int32_t spd = X_RS2MOTSPD(*X);
|
||||
if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
|
||||
}
|
||||
if(Y){
|
||||
if(!chkYs(*Y)) return MCC_E_BADFORMAT;
|
||||
int32_t spd = Y_RS2MOTSPD(*Y);
|
||||
if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED;
|
||||
}
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief move2s - move to target with given max speed
|
||||
* @param target (i) - target or NULL
|
||||
* @param speed (i) - speed or NULL
|
||||
* @return
|
||||
*/
|
||||
static mcc_errcodes_t move2s(const coords_t *target, const coords_t *speed){
|
||||
if(!target && !speed) return MCC_E_BADFORMAT;
|
||||
if(!target) return setspeed(&speed->X, &speed->Y);
|
||||
if(!speed) return move2(&target->X, &target->Y);
|
||||
if(!chkX(target->X) || !chkY(target->Y) || !chkXs(speed->X) || !chkYs(speed->Y))
|
||||
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);
|
||||
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);
|
||||
if(!SStextcmd(buf, NULL)) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
@ -89,7 +165,12 @@ static mcc_errcodes_t move2(double X, double Y){
|
||||
* @return errcode
|
||||
*/
|
||||
static mcc_errcodes_t emstop(){
|
||||
if(!SSemergStop()) return MCC_E_FAILED;
|
||||
if(!SSstop(TRUE)) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
// normal stop
|
||||
static mcc_errcodes_t stop(){
|
||||
if(!SSstop(FALSE)) return MCC_E_FAILED;
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
@ -149,13 +230,34 @@ static mcc_errcodes_t longcmd(long_command_t *cmd){
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
mcc_errcodes_t get_hwconf(hardware_configuration_t *c){
|
||||
if(!c) return MCC_E_BADFORMAT;
|
||||
SSconfig conf;
|
||||
if(!cmdC(&conf, FALSE)) return MCC_E_FAILED;
|
||||
// and bored transformations
|
||||
DBG("Xacc=%u", conf.Xconf.accel);
|
||||
DBG("Yacc=%u", conf.Yconf.accel);
|
||||
c->Xconf.accel = X_MOTACC2RS(conf.Xconf.accel);
|
||||
DBG("cacc: %g", c->Xconf.accel);
|
||||
c->Xconf.backlash = conf.Xconf.backlash;
|
||||
// ...
|
||||
c->Yconf.accel = X_MOTACC2RS(conf.Yconf.accel);
|
||||
c->Xconf.backlash = conf.Xconf.backlash;
|
||||
// ...
|
||||
return MCC_E_OK;
|
||||
}
|
||||
|
||||
// init mount class
|
||||
mount_t Mount = {
|
||||
.init = init,
|
||||
.quit = quit,
|
||||
.getMountData = getMD,
|
||||
.moveTo = move2,
|
||||
.moveWspeed = move2s,
|
||||
.setSpeed = setspeed,
|
||||
.emergStop = emstop,
|
||||
.stop = stop,
|
||||
.shortCmd = shortcmd,
|
||||
.longCmd = longcmd,
|
||||
.getHWconfig = get_hwconf,
|
||||
};
|
||||
|
||||
@ -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,13 +119,13 @@ 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
|
||||
static int getencbyte(){
|
||||
if(encfd < 0) return -1;
|
||||
uint8_t byte;
|
||||
uint8_t byte = 0;
|
||||
fd_set rfds;
|
||||
struct timeval tv;
|
||||
do{
|
||||
@ -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){
|
||||
@ -375,13 +386,14 @@ static int wr(const data_t *out, data_t *in, int needeol){
|
||||
(void) g;
|
||||
}
|
||||
}
|
||||
if(in){
|
||||
in->len = 0;
|
||||
for(size_t i = 0; i < in->maxlen; ++i){
|
||||
int b = getmntbyte();
|
||||
if(b < 0) break; // nothing to read -> go out
|
||||
in->buf[in->len++] = (uint8_t) b;
|
||||
}
|
||||
uint8_t buf[256];
|
||||
data_t dumb = {.buf = buf, .maxlen = 256};
|
||||
if(!in) in = &dumb; // even if user don't ask for answer, try to read to clear trash
|
||||
in->len = 0;
|
||||
for(size_t i = 0; i < in->maxlen; ++i){
|
||||
int b = getmntbyte();
|
||||
if(b < 0) break; // nothing to read -> go out
|
||||
in->buf[in->len++] = (uint8_t) b;
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
@ -398,6 +410,13 @@ int MountWriteRead(const data_t *out, data_t *in){
|
||||
pthread_mutex_unlock(&mntmutex);
|
||||
return ret;
|
||||
}
|
||||
// send binary data - without EOL
|
||||
int MountWriteReadRaw(const data_t *out, data_t *in){
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
int ret = wr(out, in, 0);
|
||||
pthread_mutex_unlock(&mntmutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
#ifdef EBUG
|
||||
static void logscmd(SSscmd *c){
|
||||
@ -459,3 +478,34 @@ int cmdS(SSscmd *cmd){
|
||||
int cmdL(SSlcmd *cmd){
|
||||
return bincmd((uint8_t *)cmd, sizeof(SSlcmd));
|
||||
}
|
||||
// rw == 1 to write, 0 to read
|
||||
int cmdC(SSconfig *conf, int rw){
|
||||
static data_t *wcmd = NULL, *rcmd = NULL;
|
||||
int ret = FALSE;
|
||||
// dummy buffer to clear trash in input
|
||||
char ans[300];
|
||||
data_t a = {.buf = (uint8_t*)ans, .maxlen=299};
|
||||
if(!wcmd) wcmd = cmd2dat(CMD_PROGFLASH);
|
||||
if(!rcmd) rcmd = cmd2dat(CMD_DUMPFLASH);
|
||||
pthread_mutex_lock(&mntmutex);
|
||||
if(rw){ // write
|
||||
if(!wr(wcmd, &a, 1)) goto rtn;
|
||||
}else{ // read
|
||||
data_t d;
|
||||
d.buf = (uint8_t *) conf;
|
||||
d.len = 0; d.maxlen = sizeof(SSconfig);
|
||||
ret = wr(rcmd, &d, 1);
|
||||
DBG("wr returned %s; got %zd bytes of %zd", ret ? "TRUE" : "FALSE", d.len, d.maxlen);
|
||||
if(d.len != d.maxlen) return FALSE;
|
||||
// simplest checksum
|
||||
uint16_t sum = 0;
|
||||
for(uint32_t i = 0; i < sizeof(SSconfig)-2; ++i) sum += d.buf[i];
|
||||
if(sum != conf->checksum){
|
||||
DBG("got sum: %u, need: %u", conf->checksum, sum);
|
||||
return FALSE;
|
||||
}
|
||||
}
|
||||
rtn:
|
||||
pthread_mutex_unlock(&mntmutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -36,5 +36,7 @@ int openMount(const char *path, int speed);
|
||||
void closeSerial();
|
||||
mcc_errcodes_t getMD(mountdata_t *d);
|
||||
int MountWriteRead(const data_t *out, data_t *in);
|
||||
int MountWriteReadRaw(const data_t *out, data_t *in);
|
||||
int cmdS(SSscmd *cmd);
|
||||
int cmdL(SSlcmd *cmd);
|
||||
int cmdC(SSconfig *conf, int rw);
|
||||
|
||||
@ -18,6 +18,11 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <sys/time.h>
|
||||
@ -55,8 +60,30 @@ typedef struct{
|
||||
} data_t;
|
||||
|
||||
typedef struct{
|
||||
uint8_t XBits;
|
||||
uint8_t YBits;
|
||||
uint8_t motrev :1; // If 1, the motor encoder is incremented in the opposite direction
|
||||
uint8_t motpolarity :1; // If 1, the motor polarity is reversed
|
||||
uint8_t encrev :1; // If 1, the axis encoder is reversed
|
||||
uint8_t dragtrack :1; // If 1, we are in computerless Drag and Track mode
|
||||
uint8_t trackplat :1; // If 1, we are in the tracking platform mode
|
||||
uint8_t handpaden :1; // If 1, hand paddle is enabled
|
||||
uint8_t newpad :1; // If 1, hand paddle is compatible with New hand paddle, which allows slewing in two directions and guiding
|
||||
uint8_t guidemode :1; // If 1, we are in guide mode. The pan rate is added or subtracted from the current tracking rate
|
||||
} xbits_t;
|
||||
|
||||
typedef struct{
|
||||
uint8_t motrev :1; // If 1, the motor encoder is incremented in the opposite direction
|
||||
uint8_t motpolarity :1; // If 1, the motor polarity is reversed
|
||||
uint8_t encrev :1; // If 1, the axis encoder is reversed
|
||||
/* If 1, we are in computerless Slew and Track mode
|
||||
(no clutches; use handpad to slew; must be in Drag and Track mode too) */
|
||||
uint8_t slewtrack :1;
|
||||
uint8_t digin_sens :1; // Digital input from radio handpad receiver, or RA PEC Sensor sync
|
||||
uint8_t digin :3; // Digital input from radio handpad receiver
|
||||
} ybits_t;
|
||||
|
||||
typedef struct{
|
||||
xbits_t XBits;
|
||||
ybits_t YBits;
|
||||
uint8_t ExtraBits;
|
||||
uint16_t ain0;
|
||||
uint16_t ain1;
|
||||
@ -71,8 +98,16 @@ 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{
|
||||
;
|
||||
} mountstat_t;
|
||||
|
||||
typedef struct{
|
||||
double Xmot; // 0 X motor position (rad)
|
||||
double Xspeed; // 4 X speed (rad/s)
|
||||
@ -94,15 +129,64 @@ typedef struct{
|
||||
double Yatime; // 28
|
||||
} long_command_t; // long command
|
||||
|
||||
// hardware axe configuration
|
||||
typedef struct{
|
||||
double accel; // Default Acceleration, rad/s^2
|
||||
double backlash; // Backlash (???)
|
||||
double errlimit; // Error Limit, rad
|
||||
double propgain; // Proportional Gain (???)
|
||||
double intgain; // Integral Gain (???)
|
||||
double derivgain; // Derivative Gain (???)
|
||||
double outplimit; // Output Limit, percent (0..100)
|
||||
double currlimit; // Current Limit (A)
|
||||
double intlimit; // Integral Limit (???)
|
||||
} __attribute__((packed)) axe_config_t;
|
||||
|
||||
// hardware configuration
|
||||
typedef struct{
|
||||
axe_config_t Xconf;
|
||||
xbits_t xbits;
|
||||
axe_config_t Yconf;
|
||||
ybits_t ybits;
|
||||
uint8_t address;
|
||||
double eqrate; // Equatorial Rate (???)
|
||||
double eqadj; // Equatorial UpDown adjust (???)
|
||||
double trackgoal; // Tracking Platform Goal (???)
|
||||
double latitude; // Latitude, rad
|
||||
uint32_t Ysetpr; // Azm Scope Encoder Ticks Per Rev
|
||||
uint32_t Xsetpr; // Alt Scope Encoder Ticks Per Rev
|
||||
uint32_t Ymetpr; // Azm Motor Ticks Per Rev
|
||||
uint32_t Xmetpr; // Alt Motor Ticks Per Rev
|
||||
double Xslewrate; // Alt/Dec Slew Rate (rad/s)
|
||||
double Yslewrate; // Azm/RA Slew Rate (rad/s)
|
||||
double Xpanrate; // Alt/Dec Pan Rate (rad/s)
|
||||
double Ypanrate; // Azm/RA Pan Rate (rad/s)
|
||||
double Xguiderate; // Alt/Dec Guide Rate (rad/s)
|
||||
double Yguiderate; // Azm/RA Guide Rate (rad/s)
|
||||
uint32_t baudrate; // Baud Rate (baud)
|
||||
double locsdeg; // Local Search Degrees (rad)
|
||||
double locsspeed; // Local Search Speed (rad/s)
|
||||
double backlspd; // Backlash speed (???)
|
||||
} hardware_configuration_t;
|
||||
|
||||
// mount class
|
||||
typedef struct{
|
||||
mcc_errcodes_t (*init)(conf_t *c); // init device
|
||||
void (*quit)(); // deinit
|
||||
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 (*moveTo)(const double *X, const double *Y); // move to given position ans stop
|
||||
mcc_errcodes_t (*moveWspeed)(const coords_t *target, const coords_t *speed); // move with given max speed
|
||||
mcc_errcodes_t (*setSpeed)(const double *X, const double *Y); // set speed
|
||||
mcc_errcodes_t (*stop)(); // 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
|
||||
mcc_errcodes_t (*getHWconfig)(hardware_configuration_t *c); // get hardware configuration
|
||||
} mount_t;
|
||||
|
||||
extern mount_t Mount;
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -90,6 +90,19 @@ int SStextcmd(const char *cmd, data_t *answer){
|
||||
return MountWriteRead(&d, answer);
|
||||
}
|
||||
|
||||
// the same as SStextcmd, but not adding EOL - send raw 'cmd'
|
||||
int SSrawcmd(const char *cmd, data_t *answer){
|
||||
if(!cmd){
|
||||
DBG("try to send empty command");
|
||||
return FALSE;
|
||||
}
|
||||
data_t d;
|
||||
d.buf = (uint8_t*) cmd;
|
||||
d.len = d.maxlen = strlen(cmd);
|
||||
DBG("send %zd bytes: %s", d.len, d.buf);
|
||||
return MountWriteReadRaw(&d, answer);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief SSgetint - send text command and return integer answer
|
||||
* @param cmd (i) - command to send
|
||||
@ -116,29 +129,25 @@ int SSgetint(const char *cmd, int64_t *ans){
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// commands to move X and Y to given motor position in radians; @return FALSE if failed
|
||||
// BE CAREFUL: after each poweron X and Y are 0
|
||||
// BE CAREFUL: angle isn't checking here
|
||||
int SSXmoveto(double pos){
|
||||
char buf[64];
|
||||
int64_t target = X_RAD2MOT(pos);
|
||||
DBG("move to angle %grad = %ld", pos, target);
|
||||
snprintf(buf, 63, "%s%" PRIi64, CMD_MOTX, target);
|
||||
return SStextcmd(buf, NULL);
|
||||
}
|
||||
int SSYmoveto(double pos){
|
||||
char buf[64];
|
||||
int64_t target = Y_RAD2MOT(pos);
|
||||
DBG("move to angle %grad = %ld", pos, target);
|
||||
snprintf(buf, 63, "%s%" PRIi64, CMD_MOTY, target);
|
||||
/**
|
||||
* @brief SSsetterI - integer setter
|
||||
* @param cmd - command to send
|
||||
* @param ival - value
|
||||
* @return false if failed
|
||||
*/
|
||||
int SSsetterI(const char *cmd, int32_t ival){
|
||||
char buf[128];
|
||||
snprintf(buf, 127, "%s%" PRIi32, cmd, ival);
|
||||
return SStextcmd(buf, NULL);
|
||||
}
|
||||
|
||||
int SSemergStop(){
|
||||
int SSstop(int emerg){
|
||||
int i = 0;
|
||||
const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
|
||||
const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
|
||||
for(; i < 10; ++i){
|
||||
if(!SStextcmd(CMD_EMSTOPX, NULL)) continue;
|
||||
if(SStextcmd(CMD_EMSTOPY, NULL)) break;
|
||||
if(!SStextcmd(cmdx, NULL)) continue;
|
||||
if(SStextcmd(cmdy, NULL)) break;
|
||||
}
|
||||
if(i == 10) return FALSE;
|
||||
return TRUE;
|
||||
|
||||
@ -23,92 +23,186 @@
|
||||
|
||||
#include "sidservo.h"
|
||||
|
||||
#if 0
|
||||
// ASCII commands
|
||||
#define U8P(x) ((uint8_t*)x)
|
||||
// get binary data of all statistics
|
||||
#define CMD_GETSTAT U8P("XXS")
|
||||
// send short command
|
||||
#define CMD_SHORTCMD U8P("XXR")
|
||||
// send long command
|
||||
#define CMD_LONGCMD U8P("YXR")
|
||||
/*********** base commands ***********/
|
||||
// get/set X/Y in motsteps
|
||||
#define CMD_MOTX U8P("X")
|
||||
#define CMD_MOTY U8P("Y")
|
||||
// -//- in encoders' ticks
|
||||
#define CMD_ENCX U8P("XZ")
|
||||
#define CMD_ENCY U8P("YZ")
|
||||
#define CMD_MOTX "X"
|
||||
#define CMD_MOTY "Y"
|
||||
// set X/Y position with speed "sprintf(buf, "%s%d%s%d", CMD_MOTx, tagx, CMD_MOTxS, tags)
|
||||
#define CMD_MOTXYS "S"
|
||||
// reset current motor position to given value (and stop, if moving)
|
||||
#define CMD_MOTXSET "XF"
|
||||
#define CMD_MOTYSET "YF"
|
||||
// acceleration (per each loop, max: 3900)
|
||||
#define CMD_MOTXACCEL "XR"
|
||||
#define CMD_MOTYACCEL "YR"
|
||||
// PID regulator:
|
||||
// P: 0..32767
|
||||
#define CMD_PIDPX "XP"
|
||||
#define CMD_PIDPY "YP"
|
||||
// I: 0..32767
|
||||
#define CMD_PIDIX "XI"
|
||||
#define CMD_PIDIY "YI"
|
||||
// limit of I (doesn't work): 0:24000 (WTF???)
|
||||
#define CMD_PIDILX "XL"
|
||||
#define CMD_PIDILY "YL"
|
||||
// D: 0..32767
|
||||
#define CMD_PIDDX "XD"
|
||||
#define CMD_PIDDY "YD"
|
||||
// current position error
|
||||
#define CMD_POSERRX "XE"
|
||||
#define CMD_POSERRY "YE"
|
||||
// max position error limit (X: E#, Y: e#)
|
||||
#define CMD_POSERRLIMX "XEL"
|
||||
#define CMD_POSERRLIMY "YEL"
|
||||
// current PWM output: 0..255 (or set max PWM out)
|
||||
#define CMD_PWMOUTX "XO"
|
||||
#define CMD_PWMOUTY "YO"
|
||||
// motor current *100 (or set current limit): 0..240
|
||||
#define CMD_MOTCURNTX "XC"
|
||||
#define CMD_MOTCURNTY "YC"
|
||||
// change axis to Manual mode and set the PWM output: -255:255
|
||||
#define CMD_MANUALPWMX "XM"
|
||||
#define CMD_MANUALPWMY "YM"
|
||||
// change axis to Auto mode
|
||||
#define CMD_AUTOX "XA"
|
||||
#define CMD_AUTOY "YA"
|
||||
// get positioin in encoders' ticks or reset it to given value
|
||||
#define CMD_ENCX "XZ"
|
||||
#define CMD_ENCY "YZ"
|
||||
// get/set speed (geter x: S#, getter y: s#)
|
||||
#define CMD_SPEEDX "XS"
|
||||
#define CMD_SPEEDY "YS"
|
||||
// normal stop X/Y
|
||||
#define CMD_STOPX U8P("XN")
|
||||
#define CMD_STOPY U8P("YN")
|
||||
#define CMD_STOPX "XN"
|
||||
#define CMD_STOPY "YN"
|
||||
// lower speed -> drag&track or slew&track
|
||||
#define CMD_STOPTRACKX "XNT"
|
||||
#define CMD_STOPTRACKY "YNT"
|
||||
// emergency stop
|
||||
#define CMD_EMSTOPX U8P("XG")
|
||||
#define CMD_EMSTOPY U8P("YG")
|
||||
// getters of motor's encoders per rev
|
||||
#define CMD_GETXMEPR U8P("XXU")
|
||||
#define CMD_GETYMEPR U8P("XXV")
|
||||
#define CMD_EMSTOPX "XG"
|
||||
#define CMD_EMSTOPY "YG"
|
||||
// get/set X/Ybits
|
||||
#define CMD_BITSX "XB"
|
||||
#define CMD_BITSY "YB"
|
||||
|
||||
/*********** getters/setters without "Y" variant ***********/
|
||||
// get handpad status (decimal)
|
||||
#define CMD_HANDPAD "XK"
|
||||
// get TCPU (deg F)
|
||||
#define CMD_TCPU "XH"
|
||||
// get firmware version *10
|
||||
#define CMD_FIRMVER "XV"
|
||||
// get motor voltage *10
|
||||
#define CMD_MOTVOLTAGE "XJ"
|
||||
// get/set current CPU clock (milliseconds)
|
||||
#define CMD_MILLIS "XY"
|
||||
// reset servo
|
||||
#define CMD_RESET "XQ"
|
||||
// clear to factory defaults
|
||||
#define CMD_CLRDEFAULTS "XU"
|
||||
// save configuration to flash ROM
|
||||
#define CMD_WRITEFLASH "XW"
|
||||
// read config from flash to RAM
|
||||
#define CMD_READFLASH "XT"
|
||||
// write to flash following full config (128 bytes + 2 bytes of checksum)
|
||||
#define CMD_PROGFLASH "FC"
|
||||
// read configuration (-//-)
|
||||
#define CMD_DUMPFLASH "SC"
|
||||
// get serial number
|
||||
#define CMD_SERIAL "YV"
|
||||
|
||||
/*********** extended commands ***********/
|
||||
// get/set latitute
|
||||
#define CMD_LATITUDE "XXL"
|
||||
// getters/setters of motor's encoders per rev
|
||||
#define CMD_MEPRX "XXU"
|
||||
#define CMD_MEPRY "XXV"
|
||||
// -//- axis encoders
|
||||
#define CMD_GETXAEPR U8P("XXT")
|
||||
#define CMD_GETYAEPR U8P("XXZ")
|
||||
// exit ASCII checksum mode
|
||||
#define CMD_EXITACM U8P("YXY0\r\xb8")
|
||||
#endif
|
||||
#define CMD_AEPRX "XXT"
|
||||
#define CMD_AEPRY "XXZ"
|
||||
// get/set slew rate
|
||||
#define CMD_SLEWRATEX "XXA"
|
||||
#define CMD_SLEWRATEY "XXB"
|
||||
// get/set pan rate
|
||||
#define CMD_PANRATEX "XXC"
|
||||
#define CMD_PANRATEY "XXD"
|
||||
// get/set platform tracking rate
|
||||
#define CMD_PLATRATE "XXE"
|
||||
// get/set platform up/down adjuster
|
||||
#define CMD_PLATADJ "XXF"
|
||||
// get/set platform goal
|
||||
#define CMD_PLATGOAL "XXG"
|
||||
// get/set guide rate
|
||||
#define CMD_GUIDERATEX "XXH"
|
||||
#define CMD_GUIDERATEY "XXI"
|
||||
// get/set picservo timeout (seconds)
|
||||
#define CMD_PICTMOUT "XXJ"
|
||||
// get/set digital outputs of radio handpad
|
||||
#define CMD_RADIODIGOUT "XXQ"
|
||||
// get/set argo navis mode
|
||||
#define CMD_ARGONAVIS "XXN"
|
||||
// get/set local search distance
|
||||
#define CMD_LOSCRCHDISTX "XXM"
|
||||
#define CMD_LOSCRCHDISTY "XXO"
|
||||
// get/set backlash
|
||||
#define CMD_BACKLASHX "XXO"
|
||||
#define CMD_BACKLASHY "XXP"
|
||||
|
||||
|
||||
// get binary data of all statistics
|
||||
#define CMD_GETSTAT ("XXS")
|
||||
#define CMD_GETSTAT "XXS"
|
||||
// send short command
|
||||
#define CMD_SHORTCMD ("XXR")
|
||||
#define CMD_SHORTCMD "XXR"
|
||||
// send long command
|
||||
#define CMD_LONGCMD ("YXR")
|
||||
// get/set X/Y in motsteps
|
||||
#define CMD_MOTX ("X")
|
||||
#define CMD_MOTY ("Y")
|
||||
// -//- in encoders' ticks
|
||||
#define CMD_ENCX ("XZ")
|
||||
#define CMD_ENCY ("YZ")
|
||||
// normal stop X/Y
|
||||
#define CMD_STOPX ("XN")
|
||||
#define CMD_STOPY ("YN")
|
||||
// emergency stop
|
||||
#define CMD_EMSTOPX ("XG")
|
||||
#define CMD_EMSTOPY ("YG")
|
||||
// getters of motor's encoders per rev
|
||||
#define CMD_GETXMEPR ("XXU")
|
||||
#define CMD_GETYMEPR ("XXV")
|
||||
// -//- axis encoders
|
||||
#define CMD_GETXAEPR ("XXT")
|
||||
#define CMD_GETYAEPR ("XXZ")
|
||||
#define CMD_LONGCMD "YXR"
|
||||
|
||||
|
||||
/*********** special ***********/
|
||||
// exit ASCII checksum mode
|
||||
#define CMD_EXITACM ("YXY0\r\xb8")
|
||||
#define CMD_EXITACM "YXY0\r\xb8"
|
||||
// controller status:
|
||||
// X# Y# XZ# YZ# XC# YC# V# T# X[AM] Y[AM] K#
|
||||
// X,Y - motor, XZ,YZ - encoder, XC,YC - current*100, V - voltage*10, T - temp (F), XA,YA - mode (A[uto]/M[anual]), K - handpad status bits
|
||||
#define CMD_GETSTATTEXT "\r"
|
||||
|
||||
|
||||
|
||||
// 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
|
||||
#define X_SPEED_MAX (0.17453)
|
||||
#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.))
|
||||
// motor acceleration -//-
|
||||
#define X_MOTACC2RS(n) (X_MOT2RAD(n)/65536.*1953.*1953.)
|
||||
#define X_RS2MOTACC(r) ((int32_t)(X_RAD2MOT(r)*65536./1953./1953.))
|
||||
#define Y_MOTACC2RS(n) (Y_MOT2RAD(n)/65536.*1953.*1953.)
|
||||
#define Y_RS2MOTACC(r) ((int32_t)(Y_RAD2MOT(r)*65536./1953./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.)
|
||||
@ -123,8 +217,8 @@ typedef struct{ // 41 bytes
|
||||
int32_t Xenc; // 9 Dec/HA encoder position
|
||||
int32_t Yenc; // 13
|
||||
uint8_t keypad; // 17 keypad status
|
||||
uint8_t XBits; // 18
|
||||
uint8_t YBits; // 19
|
||||
xbits_t XBits; // 18
|
||||
ybits_t YBits; // 19
|
||||
uint8_t ExtraBits; // 20
|
||||
uint16_t ain0; // 21 analog inputs
|
||||
uint16_t ain1; // 23
|
||||
@ -159,11 +253,60 @@ typedef struct{
|
||||
uint16_t checksum; // 32
|
||||
} __attribute__((packed)) SSlcmd; // long command
|
||||
|
||||
typedef struct{
|
||||
uint32_t accel; // Default Acceleration (0..3900)
|
||||
uint32_t backlash; // Backlash (???)
|
||||
uint16_t errlimit; // Error Limit (0..32767)
|
||||
uint16_t propgain; // Proportional Gain (0..32767)
|
||||
uint16_t intgain; // Integral Gain (0..32767)
|
||||
uint16_t derivgain; // Derivative Gain (0..32767)
|
||||
uint16_t outplimit; // Output Limit, 0xFF = 100.0 (0..255)
|
||||
uint16_t currlimit; // Current Limit * 100 (0..240)
|
||||
uint16_t intlimit; // Integral Limit (0..24000)
|
||||
} __attribute__((packed)) AxeConfig;
|
||||
|
||||
typedef struct{
|
||||
AxeConfig Xconf;
|
||||
xbits_t xbits;
|
||||
uint8_t unused0;
|
||||
AxeConfig Yconf;
|
||||
ybits_t ybits;
|
||||
uint8_t unused1;
|
||||
uint8_t address;
|
||||
uint8_t unused2;
|
||||
uint32_t eqrate; // Equatorial Rate (platform?) (???)
|
||||
int32_t eqadj; // Equatorial UpDown adjust (???)
|
||||
uint32_t trackgoal; // Tracking Platform Goal (???)
|
||||
uint16_t latitude; // Latitude * 100, MSB FIRST!!
|
||||
uint32_t Ysetpr; // Azm Scope Encoder Ticks Per Rev, MSB FIRST!!
|
||||
uint32_t Xsetpr; // Alt Scope Encoder Ticks Per Rev, MSB FIRST!!
|
||||
uint32_t Ymetpr; // Azm Motor Ticks Per Rev, MSB FIRST!!
|
||||
uint32_t Xmetpr; // Alt Motor Ticks Per Rev, MSB FIRST!!
|
||||
int32_t Xslewrate; // Alt/Dec Slew Rate (rates are negative in "through the pole" mode!!!)
|
||||
int32_t Yslewrate; // Azm/RA Slew Rate
|
||||
int32_t Xpanrate; // Alt/Dec Pan Rate
|
||||
int32_t Ypanrate; // Azm/RA Pan Rate
|
||||
int32_t Xguiderate; // Alt/Dec Guide Rate
|
||||
int32_t Yguiderate; // Azm/RA Guide Rate
|
||||
uint8_t unknown0; // R/A PEC Auto Sync Enable (if low bit = 1), or PicServo Comm Timeout??
|
||||
uint8_t unused3;
|
||||
uint8_t baudrate; // Baud Rate??
|
||||
uint8_t unused4;
|
||||
uint8_t specmode; // 1 = Enable Argo Navis, 2 = Enable Sky Commander
|
||||
uint8_t unused5;
|
||||
uint32_t locsdeg; // Local Search Degrees * 100
|
||||
uint32_t locsspeed; // Local Search Speed, arcsec per sec (???)
|
||||
uint32_t backlspd; // Backlash speed
|
||||
uint32_t pecticks; // RA/Azm PEC Ticks
|
||||
uint16_t unused6;
|
||||
uint16_t checksum;
|
||||
} __attribute__((packed)) SSconfig;
|
||||
|
||||
uint16_t SScalcChecksum(uint8_t *buf, int len);
|
||||
void SSconvstat(const SSstat *status, mountdata_t *mountdata, struct timeval *tdat);
|
||||
int SStextcmd(const char *cmd, data_t *answer);
|
||||
int SSrawcmd(const char *cmd, data_t *answer);
|
||||
int SSgetint(const char *cmd, int64_t *ans);
|
||||
int SSXmoveto(double pos);
|
||||
int SSYmoveto(double pos);
|
||||
int SSemergStop();
|
||||
int SSsetterI(const char *cmd, int32_t ival);
|
||||
int SSstop(int emerg);
|
||||
int SSshortCmd(SSscmd *cmd);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user