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

Compare commits

base: eddy:7363d608ea0e3e102b091a8d12b4598ed211e473
Branches Tags
eddy:main
..
compare: eddy:c862d160fe6e8c225fe3df2011b4849987f5b969
Branches Tags
eddy:main

2 Commits
7363d608ea ... c862d160fe

Author SHA1 Message Date
Edward V. Emelianov
c862d160fe update pos, statuses 2025-07-29 22:11:42 +03:00
Edward V. Emelianov
89b86f8b7f moving model 2025-07-29 13:34:17 +03:00
15 changed files with 349 additions and 85 deletions
Show Stats Expand all files Collapse all files

83
LibSidServo/PID_test/main.c
View File

@ -18,21 +18,27 @@
#include <math.h>
#include <stdio.h>
#include <strings.h>
#include <usefull_macros.h>
#include "moving.h"
// errors for states: slewing/pointing/guiding
#define MAX_POINTING_ERR (50.)
#define MAX_GUIDING_ERR (5.)
// timeout to "forget" old data from I sum array; seconds
#define PID_I_PERIOD (3.)
static movemodel_t *model = NULL;
static FILE *coordslog = NULL;
typedef enum{
Slewing,
Pointing,
Guiding
} state_t;
static state_t state = Pointing;
static state_t state = Slewing;
typedef struct{
int help;
@ -64,6 +70,9 @@ typedef struct {
double kp, ki, kd; // PID gains
double prev_error; // Previous error
double integral; // Integral term
double *pidIarray; // array for Integral
size_t pidIarrSize; // it's size
size_t curIidx; // and index of current element
} PIDController;
static PIDController pid;
@ -86,7 +95,7 @@ static sl_option_t opts[] = {
// calculate coordinate target for given time (starting from zero)
static double target_coord(double t){
if(t > 20. && t < 30.) return target_coord(20.);
double pos = 5. + 10. * sin(M_2_PI * t / 10.) + 0.2 * (drand48() - 0.5);
double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5);
return pos;
}
@ -104,23 +113,73 @@ static void pid_init(PIDController *pid, double kp, double ki, double kd) {
pid->kd = fabs(kd);
pid->prev_error = 0.;
pid->integral = 0.;
pid->curIidx = 0;
pid->pidIarrSize = PID_I_PERIOD / G.dTcorr;
if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements");
pid->pidIarray = MALLOC(double, pid->pidIarrSize);
}
static void pid_clear(PIDController *pid){
if(!pid) return;
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
pid->integral = 0.;
pid->prev_error = 0.;
pid->curIidx = 0;
}
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
double error = targcoord - p->coord, fe = fabs(error);
if(state == Pointing){
if(fe < MAX_GUIDING_ERR) state = Guiding;
return (error > 0.) ? limits.max.speed : -limits.max.speed;
}else if(fe < G.minerr) return p->speed;
pid.integral += error * dt;
switch(state){
case Slewing:
if(fe < MAX_POINTING_ERR){
pid_clear(&pid);
state = Pointing;
green("--> Pointing\n");
}else{
red("Slewing...\n");
return (error > 0.) ? limits.max.speed : -limits.max.speed;
}
break;
case Pointing:
if(fe < MAX_GUIDING_ERR){
pid_clear(&pid);
state = Guiding;
green("--> Guiding\n");
}else if(fe > MAX_POINTING_ERR){
red("--> Slewing\n");
state = Slewing;
return (error > 0.) ? limits.max.speed : -limits.max.speed;
}
break;
case Guiding:
if(fe > MAX_GUIDING_ERR){
red("--> Pointing\n");
state = Pointing;
}else if(fe < G.minerr){
green("At target\n");
//pid_clear(&pid);
//return p->speed;
}
break;
}
red("Calculate PID\n");
double oldi = pid.pidIarray[pid.curIidx], newi = error * dt;
pid.pidIarray[pid.curIidx++] = oldi;
if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0;
pid.integral += newi - oldi;
double derivative = (error - pid.prev_error) / dt;
pid.prev_error = error;
DBG("I=%g", pid.integral);
double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative) / dt / 1000.;
DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative);
double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative);
if(state == Pointing) add /= 3.;
else if(state == Guiding) add /= 7.;
DBG("ADD = %g; new speed = %g", add, p->speed + add);
return p->speed + add;
if(state == Guiding) return p->speed + add / dt / 10.;
return add / dt;
}
// ./moving -l coords -P.5 -I.05 -D1.5
// ./moving -l coords -P1.3 -D1.6
static void start_model(double Tend){
double T = 0., Tcorr = 0.;//, Tlast = 0.;
@ -167,7 +226,7 @@ int main(int argc, char **argv){
if(!coordslog) ERR("Can't open %s", G.xlog);
} else coordslog = stdout;
if(G.dTmon <= 0.) ERRX("tmon should be > 0.");
if(G.dTcorr <= 0.) ERRX("tcor should be > 0.");
if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1.");
if(G.Tend <= 0.) ERRX("tend should be > 0.");
pid_init(&pid, G.P, G.I, G.D);
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");

40
LibSidServo/PID_test/moving_model.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 15.0.1, 2025-04-14T21:38:19. -->
<!-- Written by QtCreator 17.0.0, 2025-07-29T13:32:31. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
@ -13,8 +13,8 @@
<data>
<variable>ProjectExplorer.Project.EditorSettings</variable>
<valuemap type="QVariantMap">
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
@ -92,6 +92,7 @@
<variable>ProjectExplorer.Project.Target.0</variable>
<valuemap type="QVariantMap">
<value type="QString" key="DeviceType">Desktop</value>
<value type="bool" key="HasPerBcDcs">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
@ -133,6 +134,41 @@
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">

6
LibSidServo/PID_test/plot_jpg Executable file
View File

@ -0,0 +1,6 @@
#!/usr/bin/gnuplot
set terminal jpeg size 1000,500
set output "all.jpg"
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader

5
LibSidServo/PID_test/ploterr_jpg Executable file
View File

@ -0,0 +1,5 @@
#!/usr/bin/gnuplot
set term jpeg size 1000,500
set output "error.jpg"
plot 'coords' using 1:6 with lines title columnheader

16
LibSidServo/TODO
View File

@ -1,15 +1,3 @@
1. WTF after closing?
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 484): close fd
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 489): Kill encoder thread
closeSerial (/home/eddy/C-files/LibSidServo/serial.c, line 491): close fd
*** bit out of range 0 - FD_SETSIZE on fd_set ***: terminated
Aborted
2. Init: set "motors" positions due to "encoders" position
3. From time to time: repeat 2
1. PID: slew2
2. add model & config "model ON"

4
LibSidServo/dbg.h
View File

@ -59,6 +59,6 @@ extern conf_t Conf;
fprintf(stderr, "\n");} while(0)
#else // EBUG
#define FNAME() do{}while(0)
#define DBG(...) do{}while(0)
#define FNAME()
#define DBG(...)
#endif // EBUG

82
LibSidServo/examples/SSIIconf.c
View File

@ -16,6 +16,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <math.h>
#include <stdio.h>
#include <usefull_macros.h>
@ -50,6 +51,83 @@ static sl_option_t confopts[] = {
end_option
};
static void dumpaxe(char axe, axe_config_t *c){
#define STRUCTPAR(p) (c)->p
#define DUMP(par) do{printf("%c%s=%g\n", axe, #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%c%s=%g\n", axe, #par, RAD2DEG(STRUCTPAR(par)));}while(0)
DUMPD(accel);
DUMPD(backlash);
DUMPD(errlimit);
DUMP(propgain);
DUMP(intgain);
DUMP(derivgain);
DUMP(outplimit);
DUMP(currlimit);
DUMP(intlimit);
#undef DUMP
#undef DUMPD
}
static void dumpxbits(xbits_t *c){
#define DUMPBIT(f) do{printf("X%s=%d\n", #f, STRUCTPAR(f));}while(0)
DUMPBIT(motrev);
DUMPBIT(motpolarity);
DUMPBIT(encrev);
DUMPBIT(dragtrack);
DUMPBIT(trackplat);
DUMPBIT(handpaden);
DUMPBIT(newpad);
DUMPBIT(guidemode);
#undef DUMPBIT
}
static void dumpybits(ybits_t *c){
#define DUMPBIT(f) do{printf("Y%s=%d\n", #f, STRUCTPAR(f));}while(0)
DUMPBIT(motrev);
DUMPBIT(motpolarity);
DUMPBIT(encrev);
DUMPBIT(slewtrack);
DUMPBIT(digin_sens);
printf("Ydigin=%d\n", c->digin);
#undef DUMPBIT
}
static void dumpHWconf(){
#undef STRUCTPAR
#define STRUCTPAR(p) (HW).p
#define DUMP(par) do{printf("%s=%g\n", #par, STRUCTPAR(par));}while(0)
#define DUMPD(par) do{printf("%s=%g\n", #par, RAD2DEG(STRUCTPAR(par)));}while(0)
#define DUMPU8(par) do{printf("%s=%u\n", #par, (uint8_t)STRUCTPAR(par));}while(0)
#define DUMPU32(par) do{printf("%s=%u\n", #par, (uint32_t)STRUCTPAR(par));}while(0)
green("X axe configuration:\n");
dumpaxe('X', &HW.Xconf);
green("X bits:\n");
dumpxbits(&HW.xbits);
green("Y axe configuration:\n");
dumpaxe('Y', &HW.Yconf);
green("Y bits:\n");
dumpybits(&HW.ybits);
printf("address=%d\n", HW.address);
DUMP(eqrate);
DUMP(eqadj);
DUMP(trackgoal);
DUMPD(latitude);
DUMPU32(Xsetpr);
DUMPU32(Ysetpr);
DUMPU32(Xmetpr);
DUMPU32(Ymetpr);
DUMPD(Xslewrate);
DUMPD(Yslewrate);
DUMPD(Xpanrate);
DUMPD(Ypanrate);
DUMPD(Xguiderate);
DUMPD(Yguiderate);
DUMPU32(baudrate);
DUMPD(locsdeg);
DUMPD(locsspeed);
DUMPD(backlspd);
}
int main(int argc, char** argv){
sl_init();
sl_parseargs(&argc, &argv, cmdlnopts);
@ -68,9 +146,11 @@ int main(int argc, char** argv){
green("Got configuration:\n");
printf("%s\n", c);
FREE(c);
dumpHWconf();
/*
if(G.hwconffile && G.writeconf){
;
}
}*/
Mount.quit();
return 0;
}

2
LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 17.0.0, 2025-07-23T22:48:05. -->
<!-- Written by QtCreator 17.0.0, 2025-07-29T22:07:00. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

54
LibSidServo/main.c
View File

@ -77,25 +77,7 @@ static mcc_errcodes_t init(conf_t *c){
// 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) return ret;
mountdata_t md = {0};
ret = MCC_E_FATAL;
double t0 = dtime(), t = 0.;
do{
t = dtime();
if(MCC_E_OK == getMD(&md)){
if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){
DBG("FIX motors position to encoders");
int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
DBG("All OK");
ret = MCC_E_OK;
break;
}
}
}
DBG("NO DATA; dt = %g", t - t0);
}while(t - t0 < 2.);
return ret;
return updateMotorPos();
}
// check coordinates (rad) and speeds (rad/s); return FALSE if failed
@ -120,6 +102,10 @@ static int chkYs(double s){
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
(void)target;
(void)flags;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
//...
setStat(MNT_SLEWING, MNT_SLEWING);
//...
return MCC_E_FAILED;
}
@ -133,13 +119,17 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
static mcc_errcodes_t move2(const coordpair_t *target){
if(!target) return MCC_E_BADFORMAT;
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
DBG("x,y: %g, %g", target->X, target->Y);
cmd.Xmot = target->X;
cmd.Ymot = target->Y;
cmd.Xspeed = MCC_MAX_X_SPEED;
cmd.Yspeed = MCC_MAX_Y_SPEED;
return shortcmd(&cmd);
mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setStat(MNT_SLEWING, MNT_SLEWING);
return MCC_E_OK;
}
/**
@ -167,12 +157,16 @@ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed
if(!target || !speed) return MCC_E_BADFORMAT;
if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
short_command_t cmd = {0};
cmd.Xmot = target->X;
cmd.Ymot = target->Y;
cmd.Xspeed = speed->X;
cmd.Yspeed = speed->Y;
return shortcmd(&cmd);
mcc_errcodes_t r = shortcmd(&cmd);
if(r != MCC_E_OK) return r;
setStat(MNT_SLEWING, MNT_SLEWING);
return MCC_E_OK;
}
/**
@ -266,14 +260,14 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
hwConfig->address = config.address;
// TODO: What to do with eqrate, eqadj and trackgoal?
config.latitude = __bswap_16(config.latitude);
// Convert latitude (degrees * 100 to radians)
hwConfig->latitude = (double)config.latitude / 100.0 * M_PI / 180.0;
hwConfig->latitude = ((double)config.latitude) / 100.0 * M_PI / 180.0;
// Copy ticks per revolution
hwConfig->Xsetpr = config.Xsetpr;
hwConfig->Ysetpr = config.Ysetpr;
hwConfig->Xmetpr = config.Xmetpr;
hwConfig->Ymetpr = config.Ymetpr;
hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
hwConfig->Xmetpr = __bswap_32(config.Xmetpr);
hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
// Convert slew rates (ticks per loop to rad/s)
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
@ -325,7 +319,7 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
config.xbits = hwConfig->xbits;
config.ybits = hwConfig->ybits;
// Convert latitude (radians to degrees * 100)
config.latitude = (uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0);
config.latitude = __bswap_16((uint16_t)(hwConfig->latitude * 180.0 / M_PI * 100.0));
// Convert slew rates (rad/s to ticks per loop)
config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate);
config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate);
@ -341,6 +335,10 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI);
// Convert backlash speed (rad/s to ticks per loop)
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd);
config.Xsetpr = __bswap_32(hwConfig->Xsetpr);
config.Ysetpr = __bswap_32(hwConfig->Ysetpr);
config.Xmetpr = __bswap_32(hwConfig->Xmetpr);
config.Ymetpr = __bswap_32(hwConfig->Ymetpr);
// TODO - next
(void) config;
return MCC_E_OK;

26
LibSidServo/serial.c
View File

@ -56,9 +56,19 @@ typedef struct __attribute__((packed)){
} enc_t;
/**
* @brief dtime - UNIX time with microsecond
* @return value
* @brief dtime - monotonic time from first run
* @return
*/
double dtime(){
struct timespec start_time = {0}, cur_time;
if(start_time.tv_sec == 0 && start_time.tv_nsec == 0){
clock_gettime(CLOCK_MONOTONIC, &start_time);
}
clock_gettime(CLOCK_MONOTONIC, &cur_time);
return ((double)(cur_time.tv_sec - start_time.tv_sec) +
(cur_time.tv_nsec - start_time.tv_nsec) * 1e-9);
}
#if 0
double dtime(){
double t;
struct timeval tv;
@ -66,6 +76,7 @@ double dtime(){
t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
return t;
}
#endif
#if 0
double tv2d(struct timeval *tv){
if(!tv) return 0.;
@ -522,6 +533,13 @@ mcc_errcodes_t getMD(mountdata_t *d){
return MCC_E_OK;
}
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus){
pthread_mutex_lock(&datamutex);
mountdata.Xstatus = Xstatus;
mountdata.Ystatus = Ystatus;
pthread_mutex_unlock(&datamutex);
}
// write-read without locking mutex (to be used inside other functions)
static int wr(const data_t *out, data_t *in, int needeol){
if((!out && !in) || mntfd < 0) return FALSE;
@ -598,12 +616,16 @@ static int bincmd(uint8_t *cmd, int len){
if(len == sizeof(SSscmd)){
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Short command");
#ifdef EBUG
logscmd((SSscmd*)cmd);
#endif
if(!wr(dscmd, &a, 1)) goto rtn;
}else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command");
#ifdef EBUG
loglcmd((SSlcmd*)cmd);
#endif
if(!wr(dlcmd, &a, 1)) goto rtn;
}else{
goto rtn;

1
LibSidServo/serial.h
View File

@ -35,6 +35,7 @@ int openEncoder();
int openMount();
void closeSerial();
mcc_errcodes_t getMD(mountdata_t *d);
void setStat(mnt_status_t Xstatus, mnt_status_t Ystatus);
int MountWriteRead(const data_t *out, data_t *in);
int MountWriteReadRaw(const data_t *out, data_t *in);
int cmdS(SSscmd *cmd);

19
LibSidServo/sidservo.h
View File

@ -112,12 +112,21 @@ typedef struct{
uint16_t ain1;
} extradata_t;
typedef enum{
MNT_STOPPED,
MNT_SLEWING,
MNT_POINTING,
MNT_GUIDING,
MNT_ERROR,
} mnt_status_t;
typedef struct{
mnt_status_t Xstatus;
mnt_status_t Ystatus;
coordval_t motXposition;
coordval_t motYposition;
coordval_t encXposition;
coordval_t encYposition;
// TODO: add speedX/Y
coordval_t encXspeed; // once per <config> s
coordval_t encYspeed;
uint8_t keypad;
@ -127,10 +136,6 @@ typedef struct{
double voltage;
} mountdata_t;
typedef struct{
;
} mountstat_t;
typedef struct{
double Xmot; // 0 X motor position (rad)
double Xspeed; // 4 X speed (rad/s)
@ -194,7 +199,7 @@ typedef struct{
// flags for slew function
typedef struct{
uint32_t slewNguide : 1; // ==1 to gude after slewing
uint32_t slewNguide : 1; // ==1 to guide after slewing
} slewflags_t;
// mount class
@ -203,8 +208,6 @@ typedef struct{
mcc_errcodes_t (*init)(conf_t *c); // init device
void (*quit)(); // deinit
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data
// TODO: change (or add flags) switching slew-and-stop and slew-and-track
// add mount state: stop/slew/guide
mcc_errcodes_t (*slewTo)(const coordpair_t *target, slewflags_t flags);
mcc_errcodes_t (*correctTo)(coordval_pair_t *target);
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop

48
LibSidServo/ssii.c
View File

@ -35,6 +35,28 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
return checksum;
}
static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){
if(*prev == INT32_MAX){
*stat = MNT_STOPPED;
}else if(*stat != MNT_STOPPED){
if(*prev == cur){
if(++(*nstopped) > MOTOR_STOPPED_CNT) *stat = MNT_STOPPED;
}
}else if(*prev != cur){
//*stat = MNT_SLEWING;
*nstopped = 0;
}
*prev = cur;
}
// check for stopped/pointing states
static void ChkStopped(const SSstat *s, mountdata_t *m){
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstatus);
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus);
}
/**
* @brief SSconvstat - convert stat from SSII format to human
* @param s (i) - just read data
@ -52,6 +74,7 @@ void SSconvstat(const SSstat *s, mountdata_t *m, double t){
*/
m->motXposition.val = X_MOT2RAD(s->Xmot);
m->motYposition.val = Y_MOT2RAD(s->Ymot);
ChkStopped(s, m);
m->motXposition.t = m->motYposition.t = t;
// fill encoder data from here, as there's no separate enc thread
if(!Conf.SepEncoder){
@ -154,3 +177,28 @@ int SSstop(int emerg){
return TRUE;
}
// update motors' positions due to encoders'
mcc_errcodes_t updateMotorPos(){
mountdata_t md = {0};
double t0 = dtime(), t = 0.;
DBG("start @ %g", t0);
do{
t = dtime();
if(MCC_E_OK == getMD(&md)){
DBG("got");
if(fabs(md.encXposition.t - t) < 0.1 && fabs(md.encYposition.t - t) < 0.1){
DBG("FIX motors position to encoders");
int32_t Xpos = X_RAD2MOT(md.encXposition.val), Ypos = Y_RAD2MOT(md.encYposition.val);
if(SSsetterI(CMD_MOTXSET, Xpos) && SSsetterI(CMD_MOTYSET, Ypos)){
DBG("All OK");
return MCC_E_OK;
}
}else{
DBG("on position");
return MCC_E_OK;
}
}
DBG("NO DATA; dt = %g", t - t0);
}while(t - t0 < 2.);
return MCC_E_FATAL;
}

30
LibSidServo/ssii.h
View File

@ -168,6 +168,9 @@
// Loop freq
#define SITECH_LOOP_FREQUENCY (1953.)
// amount of consequent same coordinates to detect stop
#define MOTOR_STOPPED_CNT (20)
// steps per revolution (SSI - x4 - for SSI)
// 13312000 / 4 = 3328000
#define X_MOT_STEPSPERREV_SSI (13312000.)
@ -178,9 +181,22 @@
//#define Y_MOT_STEPSPERREV (4394960.)
#define Y_MOT_STEPSPERREV (4394667.)
// convert angle in radians to +-pi
static inline double ang2half(double ang){
if(ang < -M_PI) ang += 2.*M_PI;
else if(ang > M_PI) ang -= 2.*M_PI;
return ang;
}
// convert to only positive: 0..2pi
static inline double ang2full(double ang){
if(ang < 0.) ang += 2.*M_PI;
else if(ang > 2.*M_PI) ang -= 2.*M_PI;
return ang;
}
// 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_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV)
#define Y_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / Y_MOT_STEPSPERREV)
#define 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
@ -202,15 +218,14 @@
#define X_ENC_STEPSPERREV (67108864.)
#define Y_ENC_STEPSPERREV (67108864.)
// encoder zero position
//#define X_ENC_ZERO (46033555)
#define X_ENC_ZERO (4603355)
#define Y_ENC_ZERO (36674010)
#define X_ENC_ZERO (61245239)
#define Y_ENC_ZERO (36999830)
// encoder reversed (no: +1)
#define X_ENC_SIGN (-1.)
#define Y_ENC_SIGN (-1.)
// encoder position to radians and back
#define X_ENC2RAD(n) (X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) (Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_ENC_STEPSPERREV)
#define X_ENC2RAD(n) ang2half(X_ENC_SIGN * 2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_ENC_STEPSPERREV)
#define Y_ENC2RAD(n) ang2half(Y_ENC_SIGN * 2.*M_PI * ((double)(n-Y_ENC_ZERO)) / 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))
@ -320,3 +335,4 @@ int SSgetint(const char *cmd, int64_t *ans);
int SSsetterI(const char *cmd, int32_t ival);
int SSstop(int emerg);
int SSshortCmd(SSscmd *cmd);
mcc_errcodes_t updateMotorPos();

18
moving_model/main.c
View File

@ -38,12 +38,12 @@ static pars G = {
};
static limits_t limits = {
.min = {.coord = -1e6, .speed = 0.1, .accel = 0.1},
.max = {.coord = 1e6, .speed = 1e3, .accel = 50.},
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
.max = {.coord = 1e6, .speed = 20., .accel = 9.53523},
.jerk = 10.
};
static myoption opts[] = {
static sl_option_t opts[] = {
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
{"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"},
{"deltat", NEED_ARG, NULL, 't', arg_int, APTR(&G.dT), "time interval for monitoring (microseconds, >0)"},
@ -84,9 +84,9 @@ static void monit(double tnext){
}
int main(int argc, char **argv){
initial_setup();
parseargs(&argc, &argv, opts);
if(G.help) showhelp(-1, opts);
sl_init();
sl_parseargs(&argc, &argv, opts);
if(G.help) sl_showhelp(-1, opts);
if(G.xlog){
coordslog = fopen(G.xlog, "w");
if(!coordslog) ERR("Can't open %s", G.xlog);
@ -101,8 +101,9 @@ int main(int argc, char **argv){
model = init_moving(ramp, &limits);
if(!model) ERRX("Can't init moving model: check parameters");
Tstart = nanot();
moveparam_t target = {.speed = 10., .coord = 20.};
if(move(&target)) monit(0.5);
moveparam_t target = {.speed = 8.0, .coord = 90.};
if(move(&target)) monit(60.);
/*
for(int i = 0; i < 10; ++i){
target.coord = -target.coord;
if(move(&target)) monit(1.);
@ -119,6 +120,7 @@ int main(int argc, char **argv){
target.coord = 0.; target.speed = 20.;
if(move(&target)) monit(1e6);
usleep(5000);
*/
fclose(coordslog);
return 0;
}