eddy/small_tel
1
0
Fork 0
mirror of https://github.com/eddyem/small_tel.git synced 2025-12-06 10:45:16 +03:00
Code Issues Packages Projects Releases Wiki Activity

next

Browse Source
This commit is contained in:
Edward Emelianov 2025-07-29 22:14:29 +03:00
parent e1f0a0804f
commit 46ff11df58
45 changed files with 1736 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
Auxiliary_utils/LibSidServo/PID_test/Dramp.c Normal file
View File

@ -0,0 +1,141 @@
/*
* This file is part of the moving_model 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 <math.h>
#include <stdio.h>
#include <usefull_macros.h>
#include "Dramp.h"
static movestate_t state = ST_STOP;
static moveparam_t target, Min, Max;
static double T0 = -1., Xlast0; // time when move starts, last stage starting coordinate
static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
static double T1 = -1.; // time to switch into minimal speed for best coord tolerance
typedef enum{
STAGE_NORMALSPEED,
STAGE_MINSPEED,
STAGE_STOPPED
} movingsage_t;
static movingsage_t movingstage = STAGE_STOPPED;
static int initlims(limits_t *lim){
if(!lim) return FALSE;
Min = lim->min;
Max = lim->max;
return TRUE;
}
static int calc(moveparam_t *x, double t){
DBG("target: %g, tagspeed: %g (maxspeed: %g, minspeed: %g)", x->coord, x->speed, Max.speed, Min.speed);
if(!x || t < 0.) return FALSE;
if(x->speed > Max.speed || x->speed < Min.speed || x->coord < Min.coord || x->coord > Max.coord) return FALSE;
double adist = fabs(x->coord - curparams.coord);
DBG("want dist: %g", adist);
if(adist < coord_tolerance) return TRUE; // we are at place
if(adist < time_tick * Min.speed) return FALSE; // cannot reach with current parameters
target = *x;
if(x->speed * time_tick > adist) target.speed = adist / (10. * time_tick); // take at least 10 ticks to reach position
if(target.speed < Min.speed) target.speed = Min.speed;
DBG("Approximate tag speed: %g", target.speed);
T0 = t;
// calculate time to switch into minimal speed
T1 = -1.; // no min speed phase
if(target.speed > Min.speed){
double dxpertick = target.speed * time_tick;
DBG("dX per one tick: %g", dxpertick);
double ticks_need = floor(adist / dxpertick);
DBG("ticks need: %g", ticks_need);
if(ticks_need < 1.) return FALSE; // cannot reach
if(fabs(ticks_need * dxpertick - adist) > coord_tolerance){
DBG("Need to calculate slow phase; can't reach for %g ticks at current speed", ticks_need);
double dxpersmtick = Min.speed * time_tick;
DBG("dX per smallest tick: %g", dxpersmtick);
while(--ticks_need > 1.){
double part = adist - ticks_need * dxpertick;
double smticks = floor(part / dxpersmtick);
double least = part - smticks * dxpersmtick;
if(least < coord_tolerance) break;
}
DBG("now BIG ticks: %g, T1=T0+%g", ticks_need, ticks_need*time_tick);
T1 = t + ticks_need * time_tick;
}
}
state = ST_MOVE;
Xlast0 = curparams.coord;
if(target.speed > Min.speed) movingstage = STAGE_NORMALSPEED;
else movingstage = STAGE_MINSPEED;
if(x->coord < curparams.coord) target.speed *= -1.; // real speed
curparams.speed = target.speed;
return TRUE;
}
static void stop(double _U_ t){
T0 = -1.;
curparams.accel = 0.;
curparams.speed = 0.;
state = ST_STOP;
movingstage = STAGE_STOPPED;
}
static movestate_t proc(moveparam_t *next, double t){
if(T0 < 0.) return ST_STOP;
curparams.coord = Xlast0 + (t - T0) * curparams.speed;
//DBG("coord: %g (dTmon: %g, speed: %g)", curparams.coord, t-T0, curparams.speed);
int ooops = FALSE; // oops - we are over target!
if(curparams.speed < 0.){ if(curparams.coord < target.coord) ooops = TRUE;}
else{ if(curparams.coord > target.coord) ooops = TRUE; }
if(ooops){
DBG("OOOps! We are (%g) over target (%g) -> stop", curparams.coord, target.coord);
stop(t);
if(next) *next = curparams;
return state;
}
if(movingstage == STAGE_NORMALSPEED && T1 > 0.){ // check need of T1
if(t >= T1){
DBG("T1=%g, t=%g -->", T1, t);
curparams.speed = (curparams.speed > 0.) ? Min.speed : -Min.speed;
movingstage = STAGE_MINSPEED;
Xlast0 = curparams.coord;
T0 = T1;
DBG("Go further with minimal speed");
}
}
if(fabs(curparams.coord - target.coord) < coord_tolerance){ // we are at place
DBG("OK, we are in place");
stop(t);
}
if(next) *next = curparams;
return state;
}
static movestate_t getst(moveparam_t *cur){
if(cur) *cur = curparams;
return state;
}
movemodel_t dumb = {
.init_limits = initlims,
.calculate = calc,
.proc_move = proc,
.stop = stop,
.emergency_stop = stop,
.get_state = getst,
};

23
Auxiliary_utils/LibSidServo/PID_test/Dramp.h Normal file
View File

@ -0,0 +1,23 @@
/*
* This file is part of the moving_model 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 "moving_private.h"
extern movemodel_t dumb;

57
Auxiliary_utils/LibSidServo/PID_test/Makefile Normal file
View File

@ -0,0 +1,57 @@
# run `make DEF=...` to add extra defines
PROGRAM := moving
LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all
LDFLAGS += -lusefull_macros -lm
SRCS := $(wildcard *.c)
DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111
OBJDIR := mk
CFLAGS += -O2 -Wall -Wextra -Wno-trampolines -std=gnu99
OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o))
DEPS := $(OBJS:.o=.d)
TARGFILE := $(OBJDIR)/TARGET
CC = gcc
#TARGET := RELEASE
ifeq ($(shell test -e $(TARGFILE) && echo -n yes),yes)
TARGET := $(file < $(TARGFILE))
else
TARGET := RELEASE
endif
ifeq ($(TARGET), DEBUG)
.DEFAULT_GOAL := debug
endif
release: $(PROGRAM)
debug: CFLAGS += -DEBUG -Werror
debug: TARGET := DEBUG
debug: $(PROGRAM)
$(TARGFILE): $(OBJDIR)
@echo -e "\t\tTARGET: $(TARGET)"
@echo "$(TARGET)" > $(TARGFILE)
$(PROGRAM) : $(TARGFILE) $(OBJS)
@echo -e "\t\tLD $(PROGRAM)"
$(CC) $(OBJS) $(LDFLAGS) -o $(PROGRAM)
$(OBJDIR):
@mkdir $(OBJDIR)
ifneq ($(MAKECMDGOALS),clean)
-include $(DEPS)
endif
$(OBJDIR)/%.o: %.c
@echo -e "\t\tCC $<"
$(CC) $< -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@
clean:
@echo -e "\t\tCLEAN"
@rm -rf $(OBJDIR) 2>/dev/null || true
xclean: clean
@rm -f $(PROGRAM)
.PHONY: clean xclean

27
Auxiliary_utils/LibSidServo/PID_test/Sramp.c Normal file
View File

@ -0,0 +1,27 @@
/*
* This file is part of the moving_model 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/>.
*/
// quasi non-jerk s-ramp
#include <stdio.h>
#include <usefull_macros.h>
#include "Sramp.h"
movemodel_t s_shaped = { 0 };

23
Auxiliary_utils/LibSidServo/PID_test/Sramp.h Normal file
View File

@ -0,0 +1,23 @@
/*
* This file is part of the moving_model 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 "moving_private.h"
extern movemodel_t s_shaped;

223
Auxiliary_utils/LibSidServo/PID_test/Tramp.c Normal file
View File

@ -0,0 +1,223 @@
/*
* This file is part of the moving_model 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/>.
*/
// simplest trapezioidal ramp
#include <math.h>
#include <stdio.h>
#include <strings.h>
#include <usefull_macros.h>
#include "Tramp.h"
#undef DBG
#define DBG(...)
static movestate_t state = ST_STOP;
static moveparam_t Min, Max; // `Min` acceleration not used!
typedef enum{
STAGE_ACCEL, // start from zero speed and accelerate to Max speed
STAGE_MAXSPEED, // go with target speed
STAGE_DECEL, // go from target speed to zero
STAGE_STOPPED, // stop
STAGE_AMOUNT
} movingstage_t;
static movingstage_t movingstage = STAGE_STOPPED;
static double Times[STAGE_AMOUNT] = {0}; // time when each stage starts
static moveparam_t Params[STAGE_AMOUNT] = {0}; // starting parameters for each stage
static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
static int initlims(limits_t *lim){
if(!lim) return FALSE;
Min = lim->min;
Max = lim->max;
return TRUE;
}
static void emstop(double _U_ t){
curparams.accel = 0.;
curparams.speed = 0.;
bzero(Times, sizeof(Times));
bzero(Params, sizeof(Params));
state = ST_STOP;
movingstage = STAGE_STOPPED;
}
static void stop(double t){
if(state == ST_STOP || movingstage == STAGE_STOPPED) return;
movingstage = STAGE_DECEL;
state = ST_MOVE;
Times[STAGE_DECEL] = t;
Params[STAGE_DECEL].speed = curparams.speed;
if(curparams.speed > 0.) Params[STAGE_DECEL].accel = -Max.accel;
else Params[STAGE_DECEL].accel = Max.accel;
Params[STAGE_DECEL].coord = curparams.coord;
// speed: v=v2+a2(t-t2), v2 and a2 have different signs; t3: v3=0 -> t3=t2-v2/a2
Times[STAGE_STOPPED] = t - curparams.speed / Params[STAGE_DECEL].accel;
// coordinate: x=x2+v2(t-t2)+a2(t-t2)^2/2 -> x3=x2+v2(t3-t2)+a2(t3-t2)^2/2
double dt = Times[STAGE_STOPPED] - t;
Params[STAGE_STOPPED].coord = curparams.coord + curparams.speed * dt +
Params[STAGE_DECEL].accel * dt * dt / 2.;
}
/**
* @brief calc - moving calculation
* @param x - using max speed (>0!!!) and coordinate
* @param t - current time value
* @return FALSE if can't move with given parameters
*/
static int calc(moveparam_t *x, double t){
if(!x) return FALSE;
if(x->coord < Min.coord || x->coord > Max.coord) return FALSE;
if(x->speed < Min.speed || x->speed > Max.speed) return FALSE;
double Dx = fabs(x->coord - curparams.coord); // full distance
double sign = (x->coord > curparams.coord) ? 1. : -1.; // sign of target accelerations and speeds
// we have two variants: with or without stage with constant speed
double dt23 = x->speed / Max.accel; // time of deceleration stage for given speed
double dx23 = x->speed * dt23 / 2.; // distance on dec stage (abs)
DBG("Dx=%g, sign=%g, dt23=%g, dx23=%g", Dx, sign, dt23, dx23);
double setspeed = x->speed; // new max speed (we can change it if need)
double dt01, dx01; // we'll fill them depending on starting conditions
Times[0] = t;
Params[0].speed = curparams.speed;
Params[0].coord = curparams.coord;
double curspeed = fabs(curparams.speed);
double dt0s = curspeed / Max.accel; // time of stopping phase
double dx0s = curspeed * dt0s / 2.; // distance
DBG("dt0s=%g, dx0s=%g", dt0s, dx0s);
if(dx0s > Dx){
WARNX("distance too short");
return FALSE;
}
if(fabs(Dx - dx0s) < coord_tolerance){ // just stop and we'll be on target
DBG("Distance good to just stop");
stop(t);
return TRUE;
}
if(curparams.speed * sign < 0. || state == ST_STOP){ // we should change speed sign
// after stop we will have full profile
double dxs3 = Dx - dx0s;
double newspeed = sqrt(Max.accel * dxs3);
if(newspeed < setspeed) setspeed = newspeed; // we can't reach user speed
DBG("dxs3=%g, setspeed=%g", dxs3, setspeed);
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
Params[0].accel = sign * Max.accel;
if(state == ST_STOP) dx01 = setspeed * dt01 / 2.;
else dx01 = dt01 * (dt01 / 2. * Max.accel - curspeed);
DBG("dx01=%g, dt01=%g", dx01, dt01);
}else{ // increase or decrease speed without stopping phase
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
double a = sign * Max.accel;
if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;}
//double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.;
DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01);
if(dx01 + dx23 > Dx){ // calculate max speed
setspeed = sqrt(Max.accel * Dx - curspeed * curspeed / 2.);
if(setspeed < curspeed){
setspeed = curparams.speed;
dt01 = 0.; dx01 = 0.;
Params[0].accel = 0.;
}else{
Params[0].accel = a;
dt01 = fabs(setspeed - curspeed) / Max.accel;
dx01 = curspeed * dt01 + Max.accel * dt01 * dt01 / 2.;
}
}else Params[0].accel = a;
}
if(setspeed < Min.speed){
WARNX("New speed should be too small");
return FALSE;
}
moveparam_t *p = &Params[STAGE_MAXSPEED];
p->accel = 0.; p->speed = sign * setspeed;
p->coord = curparams.coord + dx01 * sign;
Times[STAGE_MAXSPEED] = Times[0] + dt01;
dt23 = setspeed / Max.accel;
dx23 = setspeed * dt23 / 2.;
// calculate dx12 and dt12
double dx12 = Dx - dx01 - dx23;
if(dx12 < -coord_tolerance){
WARNX("Oops, WTF dx12=%g?", dx12);
return FALSE;
}
double dt12 = dx12 / setspeed;
p = &Params[STAGE_DECEL];
p->accel = -sign * Max.accel;
p->speed = sign * setspeed;
p->coord = Params[STAGE_MAXSPEED].coord + sign * dx12;
Times[STAGE_DECEL] = Times[STAGE_MAXSPEED] + dt12;
p = &Params[STAGE_STOPPED];
p->accel = 0.; p->speed = 0.; p->coord = x->coord;
Times[STAGE_STOPPED] = Times[STAGE_DECEL] + dt23;
for(int i = 0; i < 4; ++i)
DBG("%d: t=%g, coord=%g, speed=%g, accel=%g", i,
Times[i], Params[i].coord, Params[i].speed, Params[i].accel);
state = ST_MOVE;
movingstage = STAGE_ACCEL;
return TRUE;
}
static movestate_t proc(moveparam_t *next, double t){
if(state == ST_STOP) goto ret;
for(movingstage_t s = STAGE_STOPPED; s >= 0; --s){
if(Times[s] <= t){ // check time for current stage
movingstage = s;
break;
}
}
if(movingstage == STAGE_STOPPED){
curparams.coord = Params[STAGE_STOPPED].coord;
emstop(t);
goto ret;
}
// calculate current parameters
double dt = t - Times[movingstage];
double a = Params[movingstage].accel;
double v0 = Params[movingstage].speed;
double x0 = Params[movingstage].coord;
curparams.accel = a;
curparams.speed = v0 + a * dt;
curparams.coord = x0 + v0 * dt + a * dt * dt / 2.;
ret:
if(next) *next = curparams;
return state;
}
static movestate_t getst(moveparam_t *cur){
if(cur) *cur = curparams;
return state;
}
static double gettstop(){
return Times[STAGE_STOPPED];
}
movemodel_t trapez = {
.init_limits = initlims,
.stop = stop,
.emergency_stop = emstop,
.get_state = getst,
.calculate = calc,
.proc_move = proc,
.stoppedtime = gettstop,
};

23
Auxiliary_utils/LibSidServo/PID_test/Tramp.h Normal file
View File

@ -0,0 +1,23 @@
/*
* This file is part of the moving_model 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 "moving_private.h"
extern movemodel_t trapez;

243
Auxiliary_utils/LibSidServo/PID_test/main.c Normal file
View File

@ -0,0 +1,243 @@
/*
* This file is part of the moving_model 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 <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 = Slewing;
typedef struct{
int help;
char *ramptype;
char *xlog;
double dTmon;
double dTcorr;
double Tend;
double minerr;
double P, I, D;
} pars;
static pars G = {
.ramptype = "t",
.dTmon = 0.01,
.dTcorr = 0.05,
.Tend = 100.,
.minerr = 0.1,
.P = 0.8,
};
static limits_t limits = {
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
.max = {.coord = 1e6, .speed = 1e3, .accel = 500.},
.jerk = 10.
};
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;
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"},
{"tmon", NEED_ARG, NULL, 'T', arg_double, APTR(&G.dTmon), "time interval for monitoring (seconds, default: 0.001)"},
{"tcor", NEED_ARG, NULL, 't', arg_double, APTR(&G.dTcorr), "time interval for corrections (seconds, default: 0.05)"},
{"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"},
{"tend", NEED_ARG, NULL, 'e', arg_double, APTR(&G.Tend), "end time of monitoring (seconds, default: 100)"},
{"minerr", NEED_ARG, NULL, 'm', arg_double, APTR(&G.minerr), "minimal error for corrections (units, default: 0.1)"},
{"prop", NEED_ARG, NULL, 'P', arg_double, APTR(&G.P), "P-coefficient of PID"},
{"integ", NEED_ARG, NULL, 'I', arg_double, APTR(&G.I), "I-coefficient of PID"},
{"diff", NEED_ARG, NULL, 'D', arg_double, APTR(&G.D), "D-coefficient of PID"},
// TODO: add parameters for limits setting
end_option
};
// 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 = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5);
return pos;
}
/* P-only == oscillations
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
double error = targcoord - p->coord;
if(fabs(error) < G.minerr) return p->speed;
return p->speed + error / dt / 500.;
}
*/
static void pid_init(PIDController *pid, double kp, double ki, double kd) {
pid->kp = fabs(kp);
pid->ki = fabs(ki);
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);
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("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);
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.;
moveparam_t target;
while(T <= Tend){
moveparam_t p;
movestate_t st = model->get_state(&p);
if(st == ST_MOVE) st = model->proc_move(&p, T);
double nextcoord = target_coord(T);
double error = nextcoord - p.coord;
if(T - Tcorr >= G.dTcorr){ // check correction
double speed = getNewSpeed(&p, nextcoord, T - Tcorr);
target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5;
target.speed = fabs(speed);
double res_speed = limits.max.speed / 2.;
if(target.speed > limits.max.speed){
target.speed = limits.max.speed;
res_speed = limits.max.speed / 4.;
}else if(target.speed < limits.min.speed){
target.speed = limits.min.speed;
res_speed = limits.min.speed * 4.;
}
if(!move_to(&target, T)){
target.speed = res_speed;
if(!move_to(&target, T))
WARNX("move(): can't move to %g with max speed %g", target.coord, target.speed);
}
DBG("%g: tag/cur speed= %g / %g; tag/cur pos = %g / %g; err = %g", T, target.speed, p.speed, target.coord, p.coord, error);
Tcorr = T;
}
// make log
fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\t%-10.4f\n",
T, nextcoord, p.coord, p.speed, p.accel, error);
T += G.dTmon;
}
}
int main(int argc, char **argv){
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);
} else coordslog = stdout;
if(G.dTmon <= 0.) ERRX("tmon 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");
ramptype_t ramp = RAMP_AMOUNT;
if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB;
else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM;
else if(*G.ramptype == 's' || *G.ramptype == 'S') ramp = RAMP_S;
else ERRX("Point \"d\" (dumb), \"s\" (s-type), or \"t\" (trapez) for ramp type");
model = init_moving(ramp, &limits);
if(!model) ERRX("Can't init moving model: check parameters");
start_model(G.Tend);
fclose(coordslog);
return 0;
}

105
Auxiliary_utils/LibSidServo/PID_test/moving.c Normal file
View File

@ -0,0 +1,105 @@
/*
* This file is part of the moving_model 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 <math.h>
#include <stdio.h>
#include <usefull_macros.h>
#include <pthread.h>
#include <time.h>
#include "moving.h"
#include "moving_private.h"
#include "Dramp.h"
#include "Sramp.h"
#include "Tramp.h"
//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
static movemodel_t *model = NULL;
double coord_tolerance = COORD_TOLERANCE_DEFAULT;
double time_tick = TIME_TICK_DEFAULT;
// difference of time from first call, using nanoseconds
double nanot(){
static struct timespec *start = NULL;
struct timespec now;
if(!start){
start = MALLOC(struct timespec, 1);
if(!start) return -1.;
if(clock_gettime(CLOCK_REALTIME, start)) return -1.;
}
if(clock_gettime(CLOCK_REALTIME, &now)) return -1.;
//DBG("was: %ld, now: %ld", start->tv_nsec, now.tv_nsec);
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
double sd = (double)now.tv_sec - (double)start->tv_sec;
return sd + nd;
}
static void chkminmax(double *min, double *max){
if(*min <= *max) return;
double t = *min;
*min = *max;
*max = t;
}
movemodel_t *init_moving(ramptype_t type, limits_t *l){
if(!l) return FALSE;
switch(type){
case RAMP_DUMB:
model = &dumb;
break;
case RAMP_TRAPEZIUM:
model = &trapez;
break;
case RAMP_S:
model = &s_shaped;
break;
default:
return FALSE;
}
if(!model->init_limits) return NULL;
moveparam_t *max = &l->max, *min = &l->min;
if(min->speed < 0.) min->speed = -min->speed;
if(max->speed < 0.) max->speed = -max->speed;
if(min->accel < 0.) min->accel = -min->accel;
if(max->accel < 0.) max->accel = -max->accel;
chkminmax(&min->coord, &max->coord);
chkminmax(&min->speed, &max->speed);
chkminmax(&min->accel, &max->accel);
if(!model->init_limits(l)) return NULL;
return model;
}
int move_to(moveparam_t *target, double t){
if(!target || !model) return FALSE;
DBG("MOVE to %g at speed %g", target->coord, target->speed);
// only positive velocity
if(target->speed < 0.) target->speed = -target->speed;
// don't mind about acceleration - user cannot set it now
return model->calculate(target, t);
}
int init_coordtol(double tolerance){
if(tolerance < COORD_TOLERANCE_MIN || tolerance > COORD_TOLERANCE_MAX) return FALSE;
coord_tolerance = tolerance;
return TRUE;
}
int init_timetick(double tick){
if(tick < TIME_TICK_MIN || tick > TIME_TICK_MAX) return FALSE;
time_tick = tick;
return TRUE;
}

70
Auxiliary_utils/LibSidServo/PID_test/moving.h Normal file
View File

@ -0,0 +1,70 @@
/*
* This file is part of the moving_model 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
// tolerance, time ticks
#define COORD_TOLERANCE_DEFAULT (0.01)
#define COORD_TOLERANCE_MIN (0.0001)
#define COORD_TOLERANCE_MAX (10.)
#define TIME_TICK_DEFAULT (0.0001)
#define TIME_TICK_MIN (1e-9)
#define TIME_TICK_MAX (10.)
typedef enum{
RAMP_DUMB, // no ramp: infinite acceleration/deceleration
RAMP_TRAPEZIUM, // trapezium ramp
RAMP_S, // s-shaped ramp
RAMP_AMOUNT
} ramptype_t;
typedef enum{
ST_STOP, // stopped
ST_MOVE, // moving
ST_AMOUNT
} movestate_t;
typedef struct{ // all values could be both as positive and negative
double coord;
double speed;
double accel;
} moveparam_t;
typedef struct{
moveparam_t min;
moveparam_t max;
double jerk;
} limits_t;
typedef struct{
int (*init_limits)(limits_t *lim); // init values of limits, jerk
int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t
movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t
movestate_t (*get_state)(moveparam_t *cur); // get current moving state
void (*stop)(double t); // stop by ramp
void (*emergency_stop)(double t); // stop with highest acceleration
double (*stoppedtime)(); // time when moving will ends
} movemodel_t;
extern double coord_tolerance;
double nanot();
movemodel_t *init_moving(ramptype_t type, limits_t *l);
int init_coordtol(double tolerance);
int init_timetick(double tick);
int move_to(moveparam_t *target, double t);

1
Auxiliary_utils/LibSidServo/PID_test/moving_model.cflags Normal file
View File

@ -0,0 +1 @@
-std=c17

4
Auxiliary_utils/LibSidServo/PID_test/moving_model.config Normal file
View File

@ -0,0 +1,4 @@
// Add predefined macros for your project here. For example:
// #define THE_ANSWER 42
#define _XOPEN_SOURCE 666
#define EBUG

1
Auxiliary_utils/LibSidServo/PID_test/moving_model.creator Normal file
View File

@ -0,0 +1 @@
[General]

221
Auxiliary_utils/LibSidServo/PID_test/moving_model.creator.user Normal file
View File

@ -0,0 +1,221 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 17.0.0, 2025-07-29T13:32:31. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{cf63021e-ef53-49b0-b03b-2f2570cdf3b6}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
<value type="qlonglong">0</value>
</data>
<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.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
</valuemap>
</valuemap>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
<value type="QString" key="language">QmlJS</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
</valuemap>
</valuemap>
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
<value type="int" key="EditorConfiguration.IndentSize">4</value>
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">1</value>
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
<value type="int" key="EditorConfiguration.TabSize">8</value>
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.PluginSettings</variable>
<valuemap type="QVariantMap">
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
<value type="bool" key="AutoTest.Framework.Boost">true</value>
<value type="bool" key="AutoTest.Framework.CTest">false</value>
<value type="bool" key="AutoTest.Framework.Catch">true</value>
<value type="bool" key="AutoTest.Framework.GTest">true</value>
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
</valuemap>
<value type="bool" key="AutoTest.ApplyFilter">false</value>
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
<value type="int" key="AutoTest.RunAfterBuild">0</value>
<value type="bool" key="AutoTest.UseGlobal">true</value>
<valuemap type="QVariantMap" key="ClangTools">
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
<value type="int" key="ClangTools.ParallelJobs">1</value>
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
</valuemap>
</valuemap>
</data>
<data>
<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>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</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.BuildConfiguration.0">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/eddy/C-files/mountcontrol.git/moving_model</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">all</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</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.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">clean</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</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.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
<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">
<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>
</data>
<data>
<variable>ProjectExplorer.Project.TargetCount</variable>
<value type="qlonglong">1</value>
</data>
<data>
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
<value type="int">22</value>
</data>
<data>
<variable>Version</variable>
<value type="int">22</value>
</data>
</qtcreator>

184
Auxiliary_utils/LibSidServo/PID_test/moving_model.creator.user.7bd84e3 Normal file
View File

@ -0,0 +1,184 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 16.0.0, 2025-03-18T22:08:04. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
</data>
<data>
<variable>ProjectExplorer.Project.ActiveTarget</variable>
<value type="qlonglong">0</value>
</data>
<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.CamelCaseNavigation">true</value>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
<value type="QString" key="language">Cpp</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
</valuemap>
</valuemap>
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
<value type="QString" key="language">QmlJS</value>
<valuemap type="QVariantMap" key="value">
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
</valuemap>
</valuemap>
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
<value type="int" key="EditorConfiguration.IndentSize">4</value>
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
<value type="int" key="EditorConfiguration.TabSize">8</value>
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.PluginSettings</variable>
<valuemap type="QVariantMap">
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
<value type="bool" key="AutoTest.Framework.Boost">true</value>
<value type="bool" key="AutoTest.Framework.CTest">false</value>
<value type="bool" key="AutoTest.Framework.Catch">true</value>
<value type="bool" key="AutoTest.Framework.GTest">true</value>
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
</valuemap>
<value type="bool" key="AutoTest.ApplyFilter">false</value>
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
<value type="int" key="AutoTest.RunAfterBuild">0</value>
<value type="bool" key="AutoTest.UseGlobal">true</value>
<valuemap type="QVariantMap" key="ClangTools">
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
<value type="int" key="ClangTools.ParallelJobs">8</value>
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
</valuemap>
</valuemap>
</data>
<data>
<variable>ProjectExplorer.Project.Target.0</variable>
<valuemap type="QVariantMap">
<value type="QString" key="DeviceType">Desktop</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">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</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.BuildConfiguration.0">
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/moving_model</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">all</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
</valuemap>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
<value type="QString">clean</value>
</valuelist>
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
</valuemap>
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</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">Deploy</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</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="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>
</data>
<data>
<variable>ProjectExplorer.Project.TargetCount</variable>
<value type="qlonglong">1</value>
</data>
<data>
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
<value type="int">22</value>
</data>
<data>
<variable>Version</variable>
<value type="int">22</value>
</data>
</qtcreator>

1
Auxiliary_utils/LibSidServo/PID_test/moving_model.cxxflags Normal file
View File

@ -0,0 +1 @@
-std=c++17

10
Auxiliary_utils/LibSidServo/PID_test/moving_model.files Normal file
View File

@ -0,0 +1,10 @@
Dramp.c
Dramp.h
Sramp.c
Sramp.h
Tramp.c
Tramp.h
main.c
moving.c
moving.h
moving_private.h

0
Auxiliary_utils/LibSidServo/PID_test/moving_model.includes Normal file
View File

26
Auxiliary_utils/LibSidServo/PID_test/moving_private.h Normal file
View File

@ -0,0 +1,26 @@
/*
* This file is part of the moving_model 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 "moving.h"
extern double coord_tolerance;
extern double time_tick;

4
Auxiliary_utils/LibSidServo/PID_test/plot Executable file
View File

@ -0,0 +1,4 @@
#!/usr/bin/gnuplot
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
pause mouse

8
Auxiliary_utils/LibSidServo/PID_test/plot.cont Executable file
View File

@ -0,0 +1,8 @@
#!/usr/bin/gnuplot
#set term pdf
#set output "output.pdf"
while(1){
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
pause 1
}

6
Auxiliary_utils/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
Auxiliary_utils/LibSidServo/PID_test/plot_pdf Executable file
View File

@ -0,0 +1,5 @@
#!/usr/bin/gnuplot
set term pdf
set output "output.pdf"
plot for [col=2:4] 'coordlog' using 1:col with lines title columnheader

4
Auxiliary_utils/LibSidServo/PID_test/plotacc Executable file
View File

@ -0,0 +1,4 @@
#!/usr/bin/gnuplot
plot 'coords' using 1:5 with lines title columnheader
pause mouse

4
Auxiliary_utils/LibSidServo/PID_test/ploterr Executable file
View File

@ -0,0 +1,4 @@
#!/usr/bin/gnuplot
plot 'coords' using 1:6 with lines title columnheader
pause mouse

6
Auxiliary_utils/LibSidServo/PID_test/ploterr.cont Executable file
View File

@ -0,0 +1,6 @@
#!/usr/bin/gnuplot
while(1){
plot 'coords' using 1:6 with lines title columnheader
pause 1
}

5
Auxiliary_utils/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
Auxiliary_utils/LibSidServo/TODO
View File

@ -1,15 +1,3 @@
1. WTF after closing? 1. PID: slew2
2. add model & config "model ON"
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

4
Auxiliary_utils/LibSidServo/dbg.h
View File

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

82
Auxiliary_utils/LibSidServo/examples/SSIIconf.c
View File

@ -16,6 +16,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <math.h>
#include <stdio.h> #include <stdio.h>
#include <usefull_macros.h> #include <usefull_macros.h>
@ -50,6 +51,83 @@ static sl_option_t confopts[] = {
end_option 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){ int main(int argc, char** argv){
sl_init(); sl_init();
sl_parseargs(&argc, &argv, cmdlnopts); sl_parseargs(&argc, &argv, cmdlnopts);
@ -68,9 +146,11 @@ int main(int argc, char** argv){
green("Got configuration:\n"); green("Got configuration:\n");
printf("%s\n", c); printf("%s\n", c);
FREE(c); FREE(c);
dumpHWconf();
/*
if(G.hwconffile && G.writeconf){ if(G.hwconffile && G.writeconf){
; ;
} }*/
Mount.quit(); Mount.quit();
return 0; return 0;
} }

4
Auxiliary_utils/LibSidServo/examples/dump.c
View File

@ -184,8 +184,8 @@ void chk0(int ncycles){
if(!getPos(&M, NULL)) signals(2); if(!getPos(&M, NULL)) signals(2);
if(M.X.val || M.Y.val){ if(M.X.val || M.Y.val){
WARNX("Mount position isn't @ zero; moving"); WARNX("Mount position isn't @ zero; moving");
double zero = 0.; coordpair_t zero = {0., 0.};
Mount.moveTo(&zero, &zero); Mount.moveTo(&zero);
waitmoving(ncycles); waitmoving(ncycles);
green("Now mount @ zero\n"); green("Now mount @ zero\n");
} }

7
Auxiliary_utils/LibSidServo/examples/dumpmoving.c
View File

@ -92,11 +92,12 @@ int main(int argc, char **argv){
signal(SIGINT, signals); // ctrl+C - quit signal(SIGINT, signals); // ctrl+C - quit
signal(SIGQUIT, signals); // ctrl+\ - quit signal(SIGQUIT, signals); // ctrl+\ - quit
signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z signal(SIGTSTP, SIG_IGN); // ignore ctrl+Z
double tagx = DEG2RAD(45.) + M.X.val, tagy = DEG2RAD(45.) + M.Y.val; coordpair_t tag = {.X = DEG2RAD(45.) + M.X.val, .Y = DEG2RAD(45.) + M.Y.val};
if(MCC_E_OK != Mount.moveTo(&tagx, &tagy)) if(MCC_E_OK != Mount.moveTo(&tag))
ERRX("Can't move to 45, 45"); ERRX("Can't move to 45, 45");
dumpmoving(fcoords, 30., G.Ncycles); dumpmoving(fcoords, 30., G.Ncycles);
Mount.moveTo(&M.X.val, &M.Y.val); tag.X = M.X.val; tag.Y = M.Y.val;
Mount.moveTo(&tag);
dumpmoving(fcoords, 30., G.Ncycles); dumpmoving(fcoords, 30., G.Ncycles);
signals(0); signals(0);
return 0; return 0;

3
Auxiliary_utils/LibSidServo/examples/dumpmoving_scmd.c
View File

@ -168,7 +168,8 @@ int main(int argc, char **argv){
// and go back with 7deg/s // and go back with 7deg/s
move(0., 0., 7.); move(0., 0., 7.);
// be sure to move @ starting position // be sure to move @ starting position
Mount.moveTo(&M.X.val, &M.Y.val); coordpair_t tag = {.X = M.X.val, .Y = M.Y.val};
Mount.moveTo(&tag);
// wait moving ends // wait moving ends
pthread_join(dthr, NULL); pthread_join(dthr, NULL);
signals(0); signals(0);

11
Auxiliary_utils/LibSidServo/examples/dumpswing.c
View File

@ -146,24 +146,25 @@ int main(int argc, char **argv){
tagX = 0.; tagY = DEG2RAD(G.amplitude); tagX = 0.; tagY = DEG2RAD(G.amplitude);
} }
double t = Mount.currentT(), t0 = t; double t = Mount.currentT(), t0 = t;
double divide = 2., rtagX = -tagX, rtagY = -tagY; coordpair_t tag = {.X = tagX, .Y = tagY}, rtag = {.X = -tagX, .Y = -tagY};
double divide = 2.;
for(int i = 0; i < G.Nswings; ++i){ for(int i = 0; i < G.Nswings; ++i){
Mount.moveTo(&tagX, &tagY); Mount.moveTo(&tag);
DBG("CMD: %g", Mount.currentT()-t0); DBG("CMD: %g", Mount.currentT()-t0);
t += G.period / divide; t += G.period / divide;
divide = 1.; divide = 1.;
waithalf(t); waithalf(t);
DBG("Moved to +, t=%g", t-t0); DBG("Moved to +, t=%g", t-t0);
DBG("CMD: %g", Mount.currentT()-t0); DBG("CMD: %g", Mount.currentT()-t0);
Mount.moveTo(&rtagX, &rtagY); Mount.moveTo(&rtag);
t += G.period; t += G.period;
waithalf(t); waithalf(t);
DBG("Moved to -, t=%g", t-t0); DBG("Moved to -, t=%g", t-t0);
DBG("CMD: %g", Mount.currentT()-t0); DBG("CMD: %g", Mount.currentT()-t0);
} }
double zero = 0.; tag = (coordpair_t){.X = 0., .Y = 0.};
// be sure to move @ 0,0 // be sure to move @ 0,0
Mount.moveTo(&zero, &zero); Mount.moveTo(&tag);
// wait moving ends // wait moving ends
pthread_join(dthr, NULL); pthread_join(dthr, NULL);
#undef SCMD #undef SCMD

41
Auxiliary_utils/LibSidServo/examples/goto.c
View File

@ -88,8 +88,8 @@ int main(int _U_ argc, char _U_ **argv){
return 1; return 1;
} }
if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount"); if(MCC_E_OK != Mount.init(Config)) ERRX("Can't init mount");
coordval_pair_t M; coordval_pair_t M, E;
if(!getPos(&M, NULL)) ERRX("Can't get current position"); if(!getPos(&M, &E)) ERRX("Can't get current position");
if(G.coordsoutput){ if(G.coordsoutput){
if(!G.wait) green("When logging I should wait until moving ends; added '-w'"); if(!G.wait) green("When logging I should wait until moving ends; added '-w'");
G.wait = 1; G.wait = 1;
@ -100,28 +100,27 @@ int main(int _U_ argc, char _U_ **argv){
logmnt(fcoords, NULL); logmnt(fcoords, NULL);
if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread"); if(pthread_create(&dthr, NULL, dumping, NULL)) ERRX("Can't run dump thread");
} }
printf("Mount position: X=%g, Y=%g\n", RAD2DEG(M.X.val), RAD2DEG(M.Y.val)); M.X.val = RAD2DEG(M.X.val);
M.Y.val = RAD2DEG(M.Y.val);
printf("Mount position: X=%g, Y=%g; encoders: X=%g, Y=%g\n", M.X.val, M.Y.val,
RAD2DEG(E.X.val), RAD2DEG(E.Y.val));
if(isnan(G.X) && isnan(G.Y)) goto out; if(isnan(G.X) && isnan(G.Y)) goto out;
double *xtag = NULL, *ytag = NULL, xr, yr; coordpair_t tag;
double _7deg = RAD2DEG(7.); if(isnan(G.X)){
if(!isnan(G.X)){ if(G.relative) G.X = 0.;
xr = DEG2RAD(G.X); else G.X = M.X.val;
if(G.relative) xr += M.X.val;
xtag = &xr;
// set max speed
Mount.setSpeed(&_7deg, NULL);
} }
if(!isnan(G.Y)){ if(isnan(G.Y)){
yr = DEG2RAD(G.Y); if(G.relative) G.Y = 0.;
if(G.relative) yr += M.Y.val; else G.Y = M.Y.val;
ytag = &yr;
Mount.setSpeed(NULL, &_7deg);
} }
printf("Moving to "); if(G.relative){
if(xtag) printf("X=%gdeg ", G.X); G.X += M.X.val;
if(ytag) printf("Y=%gdeg", G.Y); G.Y += M.Y.val;
printf("\n"); }
Mount.moveTo(xtag, ytag); printf("Moving to X=%gdeg, Y=%gdeg\n", G.X, G.Y);
tag.X = DEG2RAD(G.X); tag.Y = DEG2RAD(G.Y);
Mount.moveTo(&tag);
if(G.wait){ if(G.wait){
sleep(1); sleep(1);
waitmoving(G.Ncycles); waitmoving(G.Ncycles);

2
Auxiliary_utils/LibSidServo/libsidservo.config
View File

@ -1,6 +1,6 @@
// Add predefined macros for your project here. For example: // Add predefined macros for your project here. For example:
// #define THE_ANSWER 42 // #define THE_ANSWER 42
#define EBUG #define EBUG
// #define _POSIX_C_SOURCE 111 #define _POSIX_C_SOURCE 111
#define PACKAGE_VERSION "0.0.1" #define PACKAGE_VERSION "0.0.1"

2
Auxiliary_utils/LibSidServo/libsidservo.creator.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!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> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>

103
Auxiliary_utils/LibSidServo/main.c
View File

@ -26,6 +26,8 @@
conf_t Conf = {0}; conf_t Conf = {0};
static mcc_errcodes_t shortcmd(short_command_t *cmd);
/** /**
* @brief quit - close all opened and return to default state * @brief quit - close all opened and return to default state
*/ */
@ -74,11 +76,11 @@ static mcc_errcodes_t init(conf_t *c){
data_t d = {.buf = buf, .len = 0, .maxlen = 1024}; data_t d = {.buf = buf, .len = 0, .maxlen = 1024};
// read input data as there may be some trash on start // read input data as there may be some trash on start
if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED; if(!SSrawcmd(CMD_EXITACM, &d)) ret = MCC_E_FAILED;
if(ret != MCC_E_OK) quit(); if(ret != MCC_E_OK) return ret;
return ret; return updateMotorPos();
} }
// check coordinates and speeds; return FALSE if failed // check coordinates (rad) and speeds (rad/s); return FALSE if failed
// TODO fix to real limits!!! // TODO fix to real limits!!!
static int chkX(double X){ static int chkX(double X){
if(X > 2.*M_PI || X < -2.*M_PI) return FALSE; if(X > 2.*M_PI || X < -2.*M_PI) return FALSE;
@ -89,17 +91,21 @@ static int chkY(double Y){
return TRUE; return TRUE;
} }
static int chkXs(double s){ static int chkXs(double s){
if(s < 0. || s > X_SPEED_MAX) return FALSE; if(s < 0. || s > MCC_MAX_X_SPEED) return FALSE;
return TRUE; return TRUE;
} }
static int chkYs(double s){ static int chkYs(double s){
if(s < 0. || s > Y_SPEED_MAX) return FALSE; if(s < 0. || s > MCC_MAX_Y_SPEED) return FALSE;
return TRUE; return TRUE;
} }
static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
(void)target; (void)target;
(void)flags; (void)flags;
if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
//...
setStat(MNT_SLEWING, MNT_SLEWING);
//...
return MCC_E_FAILED; return MCC_E_FAILED;
} }
@ -110,41 +116,34 @@ static mcc_errcodes_t slew2(const coordpair_t *target, slewflags_t flags){
* @param Y - new Y coordinate (radians: -pi..pi) or NULL * @param Y - new Y coordinate (radians: -pi..pi) or NULL
* @return error code * @return error code
*/ */
static mcc_errcodes_t move2(const double *X, const double *Y){ static mcc_errcodes_t move2(const coordpair_t *target){
if(!X && !Y) return MCC_E_BADFORMAT; if(!target) return MCC_E_BADFORMAT;
if(X){ if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
if(!chkX(*X)) return MCC_E_BADFORMAT; if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
int32_t tag = X_RAD2MOT(*X); short_command_t cmd = {0};
DBG("X: %g, tag: %d", *X, tag); DBG("x,y: %g, %g", target->X, target->Y);
if(!SSsetterI(CMD_MOTX, tag)) return MCC_E_FAILED; cmd.Xmot = target->X;
} cmd.Ymot = target->Y;
if(Y){ cmd.Xspeed = MCC_MAX_X_SPEED;
if(!chkY(*Y)) return MCC_E_BADFORMAT; cmd.Yspeed = MCC_MAX_Y_SPEED;
int32_t tag = Y_RAD2MOT(*Y); mcc_errcodes_t r = shortcmd(&cmd);
DBG("Y: %g, tag: %d", *Y, tag); if(r != MCC_E_OK) return r;
if(!SSsetterI(CMD_MOTY, tag)) return MCC_E_FAILED; setStat(MNT_SLEWING, MNT_SLEWING);
}
return MCC_E_OK; return MCC_E_OK;
} }
/** /**
* @brief setspeed - set maximal speed over axis * @brief setspeed - set maximal speed over axis by text command
* @param X (i) - max speed or NULL * @param X (i) - max speed or NULL
* @param Y (i) - -//- * @param Y (i) - -//-
* @return errcode * @return errcode
*/ */
static mcc_errcodes_t setspeed(const double *X, const double *Y){ static mcc_errcodes_t setspeed(const coordpair_t *tagspeed){
if(!X && !Y) return MCC_E_BADFORMAT; if(!tagspeed || !chkXs(tagspeed->X) || !chkYs(tagspeed->Y)) return MCC_E_BADFORMAT;
if(X){ int32_t spd = X_RS2MOTSPD(tagspeed->X);
if(!chkXs(*X)) return MCC_E_BADFORMAT;
int32_t spd = X_RS2MOTSPD(*X);
if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED; if(!SSsetterI(CMD_SPEEDX, spd)) return MCC_E_FAILED;
} spd = Y_RS2MOTSPD(tagspeed->Y);
if(Y){
if(!chkYs(*Y)) return MCC_E_BADFORMAT;
int32_t spd = Y_RS2MOTSPD(*Y);
if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED; if(!SSsetterI(CMD_SPEEDY, spd)) return MCC_E_FAILED;
}
return MCC_E_OK; return MCC_E_OK;
} }
@ -155,18 +154,18 @@ static mcc_errcodes_t setspeed(const double *X, const double *Y){
* @return * @return
*/ */
static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){ static mcc_errcodes_t move2s(const coordpair_t *target, const coordpair_t *speed){
if(!target && !speed) return MCC_E_BADFORMAT; if(!target || !speed) return MCC_E_BADFORMAT;
if(!target) return setspeed(&speed->X, &speed->Y); if(!chkX(target->X) || !chkY(target->Y)) return MCC_E_BADFORMAT;
if(!speed) return move2(&target->X, &target->Y); if(!chkXs(speed->X) || !chkYs(speed->Y)) return MCC_E_BADFORMAT;
if(!chkX(target->X) || !chkY(target->Y) || !chkXs(speed->X) || !chkYs(speed->Y)) if(MCC_E_OK != updateMotorPos()) return MCC_E_FAILED;
return MCC_E_BADFORMAT; short_command_t cmd = {0};
char buf[128]; cmd.Xmot = target->X;
int32_t spd = X_RS2MOTSPD(speed->X), tag = X_RAD2MOT(target->X); cmd.Ymot = target->Y;
snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, CMD_MOTX, tag, CMD_MOTXYS, spd); cmd.Xspeed = speed->X;
if(!SStextcmd(buf, NULL)) return MCC_E_FAILED; cmd.Yspeed = speed->Y;
spd = Y_RS2MOTSPD(speed->Y); tag = Y_RAD2MOT(target->Y); mcc_errcodes_t r = shortcmd(&cmd);
snprintf(buf, 127, "%s%" PRIi32 "%s%" PRIi32, CMD_MOTY, tag, CMD_MOTXYS, spd); if(r != MCC_E_OK) return r;
if(!SStextcmd(buf, NULL)) return MCC_E_FAILED; setStat(MNT_SLEWING, MNT_SLEWING);
return MCC_E_OK; return MCC_E_OK;
} }
@ -192,7 +191,7 @@ static mcc_errcodes_t stop(){
static mcc_errcodes_t shortcmd(short_command_t *cmd){ static mcc_errcodes_t shortcmd(short_command_t *cmd){
if(!cmd) return MCC_E_BADFORMAT; if(!cmd) return MCC_E_BADFORMAT;
SSscmd s = {0}; SSscmd s = {0};
DBG("xmot=%g, ymot=%g", cmd->Xmot, cmd->Ymot); DBG("tag: xmot=%g rad, ymot=%g rad", cmd->Xmot, cmd->Ymot);
s.Xmot = X_RAD2MOT(cmd->Xmot); s.Xmot = X_RAD2MOT(cmd->Xmot);
s.Ymot = Y_RAD2MOT(cmd->Ymot); s.Ymot = Y_RAD2MOT(cmd->Ymot);
s.Xspeed = X_RS2MOTSPD(cmd->Xspeed); s.Xspeed = X_RS2MOTSPD(cmd->Xspeed);
@ -261,14 +260,14 @@ static mcc_errcodes_t get_hwconf(hardware_configuration_t *hwConfig){
hwConfig->address = config.address; hwConfig->address = config.address;
// TODO: What to do with eqrate, eqadj and trackgoal? // TODO: What to do with eqrate, eqadj and trackgoal?
config.latitude = __bswap_16(config.latitude);
// Convert latitude (degrees * 100 to radians) // 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 // Copy ticks per revolution
hwConfig->Xsetpr = config.Xsetpr; hwConfig->Xsetpr = __bswap_32(config.Xsetpr);
hwConfig->Ysetpr = config.Ysetpr; hwConfig->Ysetpr = __bswap_32(config.Ysetpr);
hwConfig->Xmetpr = config.Xmetpr; hwConfig->Xmetpr = __bswap_32(config.Xmetpr);
hwConfig->Ymetpr = config.Ymetpr; hwConfig->Ymetpr = __bswap_32(config.Ymetpr);
// Convert slew rates (ticks per loop to rad/s) // Convert slew rates (ticks per loop to rad/s)
hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate); hwConfig->Xslewrate = X_MOTSPD2RS(config.Xslewrate);
hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate); hwConfig->Yslewrate = Y_MOTSPD2RS(config.Yslewrate);
@ -320,7 +319,7 @@ static mcc_errcodes_t write_hwconf(hardware_configuration_t *hwConfig){
config.xbits = hwConfig->xbits; config.xbits = hwConfig->xbits;
config.ybits = hwConfig->ybits; config.ybits = hwConfig->ybits;
// Convert latitude (radians to degrees * 100) // 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) // Convert slew rates (rad/s to ticks per loop)
config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate); config.Xslewrate = X_RS2MOTSPD(hwConfig->Xslewrate);
config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate); config.Yslewrate = Y_RS2MOTSPD(hwConfig->Yslewrate);
@ -336,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); config.locsspeed = (uint32_t)(hwConfig->locsspeed * 180.0 * 3600.0 / M_PI);
// Convert backlash speed (rad/s to ticks per loop) // Convert backlash speed (rad/s to ticks per loop)
config.backlspd = X_RS2MOTSPD(hwConfig->backlspd); 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 // TODO - next
(void) config; (void) config;
return MCC_E_OK; return MCC_E_OK;

7
Auxiliary_utils/LibSidServo/moving_model/Tramp.c
View File

@ -122,7 +122,10 @@ static int calc(moveparam_t *x, double t){
DBG("dx01=%g, dt01=%g", dx01, dt01); DBG("dx01=%g, dt01=%g", dx01, dt01);
}else{ // increase or decrease speed without stopping phase }else{ // increase or decrease speed without stopping phase
dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel; dt01 = fabs(sign*setspeed - curparams.speed) / Max.accel;
double a = (curspeed > setspeed) ? -Max.accel : Max.accel; double a = sign * Max.accel;
if(sign * curparams.speed < 0.){DBG("change direction"); a = -a;}
else if(curspeed > setspeed){ DBG("lower speed @ this direction"); a = -a;}
//double a = (curspeed > setspeed) ? -Max.accel : Max.accel;
dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.; dx01 = curspeed * dt01 + a * dt01 * dt01 / 2.;
DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01); DBG("dt01=%g, a=%g, dx01=%g", dt01, a, dx01);
if(dx01 + dx23 > Dx){ // calculate max speed if(dx01 + dx23 > Dx){ // calculate max speed
@ -150,7 +153,7 @@ static int calc(moveparam_t *x, double t){
dx23 = setspeed * dt23 / 2.; dx23 = setspeed * dt23 / 2.;
// calculate dx12 and dt12 // calculate dx12 and dt12
double dx12 = Dx - dx01 - dx23; double dx12 = Dx - dx01 - dx23;
if(dx12 < 0.){ if(dx12 < -coord_tolerance){
DBG("Oops, WTF?"); DBG("Oops, WTF?");
return FALSE; return FALSE;
} }

18
Auxiliary_utils/LibSidServo/moving_model/main.c
View File

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

30
Auxiliary_utils/LibSidServo/serial.c
View File

@ -56,9 +56,19 @@ typedef struct __attribute__((packed)){
} enc_t; } enc_t;
/** /**
* @brief dtime - UNIX time with microsecond * @brief dtime - monotonic time from first run
* @return value * @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 dtime(){
double t; double t;
struct timeval tv; struct timeval tv;
@ -66,6 +76,7 @@ double dtime(){
t = tv.tv_sec + ((double)tv.tv_usec)/1e6; t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
return t; return t;
} }
#endif
#if 0 #if 0
double tv2d(struct timeval *tv){ double tv2d(struct timeval *tv){
if(!tv) return 0.; if(!tv) return 0.;
@ -492,6 +503,8 @@ void closeSerial(){
if(mntfd > -1){ if(mntfd > -1){
DBG("Kill mount thread"); DBG("Kill mount thread");
pthread_cancel(mntthread); pthread_cancel(mntthread);
DBG("join mount thread");
pthread_join(mntthread, NULL);
DBG("close fd"); DBG("close fd");
close(mntfd); close(mntfd);
mntfd = -1; mntfd = -1;
@ -499,6 +512,8 @@ void closeSerial(){
if(encfd[0] > -1){ if(encfd[0] > -1){
DBG("Kill encoder thread"); DBG("Kill encoder thread");
pthread_cancel(encthread); pthread_cancel(encthread);
DBG("join encoder thread");
pthread_join(encthread, NULL);
DBG("close fd"); DBG("close fd");
close(encfd[0]); close(encfd[0]);
encfd[0] = -1; encfd[0] = -1;
@ -518,6 +533,13 @@ mcc_errcodes_t getMD(mountdata_t *d){
return MCC_E_OK; 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) // write-read without locking mutex (to be used inside other functions)
static int wr(const data_t *out, data_t *in, int needeol){ static int wr(const data_t *out, data_t *in, int needeol){
if((!out && !in) || mntfd < 0) return FALSE; if((!out && !in) || mntfd < 0) return FALSE;
@ -594,12 +616,16 @@ static int bincmd(uint8_t *cmd, int len){
if(len == sizeof(SSscmd)){ if(len == sizeof(SSscmd)){
((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); ((SSscmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Short command"); DBG("Short command");
#ifdef EBUG
logscmd((SSscmd*)cmd); logscmd((SSscmd*)cmd);
#endif
if(!wr(dscmd, &a, 1)) goto rtn; if(!wr(dscmd, &a, 1)) goto rtn;
}else if(len == sizeof(SSlcmd)){ }else if(len == sizeof(SSlcmd)){
((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2); ((SSlcmd*)cmd)->checksum = SScalcChecksum(cmd, len-2);
DBG("Long command"); DBG("Long command");
#ifdef EBUG
loglcmd((SSlcmd*)cmd); loglcmd((SSlcmd*)cmd);
#endif
if(!wr(dlcmd, &a, 1)) goto rtn; if(!wr(dlcmd, &a, 1)) goto rtn;
}else{ }else{
goto rtn; goto rtn;

1
Auxiliary_utils/LibSidServo/serial.h
View File

@ -35,6 +35,7 @@ int openEncoder();
int openMount(); int openMount();
void closeSerial(); void closeSerial();
mcc_errcodes_t getMD(mountdata_t *d); 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 MountWriteRead(const data_t *out, data_t *in);
int MountWriteReadRaw(const data_t *out, data_t *in); int MountWriteReadRaw(const data_t *out, data_t *in);
int cmdS(SSscmd *cmd); int cmdS(SSscmd *cmd);

28
Auxiliary_utils/LibSidServo/sidservo.h
View File

@ -27,6 +27,10 @@ extern "C"
#include <stdint.h> #include <stdint.h>
#include <sys/time.h> #include <sys/time.h>
// max speeds (rad/s): xs=10 deg/s, ys=8 deg/s
#define MCC_MAX_X_SPEED (0.174533)
#define MCC_MAX_Y_SPEED (0.139626)
// max speed interval, seconds // max speed interval, seconds
#define MCC_CONF_MAX_SPEEDINT (2.) #define MCC_CONF_MAX_SPEEDINT (2.)
// minimal speed interval in parts of EncoderReqInterval // minimal speed interval in parts of EncoderReqInterval
@ -108,12 +112,21 @@ typedef struct{
uint16_t ain1; uint16_t ain1;
} extradata_t; } extradata_t;
typedef enum{
MNT_STOPPED,
MNT_SLEWING,
MNT_POINTING,
MNT_GUIDING,
MNT_ERROR,
} mnt_status_t;
typedef struct{ typedef struct{
mnt_status_t Xstatus;
mnt_status_t Ystatus;
coordval_t motXposition; coordval_t motXposition;
coordval_t motYposition; coordval_t motYposition;
coordval_t encXposition; coordval_t encXposition;
coordval_t encYposition; coordval_t encYposition;
// TODO: add speedX/Y
coordval_t encXspeed; // once per <config> s coordval_t encXspeed; // once per <config> s
coordval_t encYspeed; coordval_t encYspeed;
uint8_t keypad; uint8_t keypad;
@ -123,10 +136,6 @@ typedef struct{
double voltage; double voltage;
} mountdata_t; } mountdata_t;
typedef struct{
;
} mountstat_t;
typedef struct{ typedef struct{
double Xmot; // 0 X motor position (rad) double Xmot; // 0 X motor position (rad)
double Xspeed; // 4 X speed (rad/s) double Xspeed; // 4 X speed (rad/s)
@ -190,7 +199,7 @@ typedef struct{
// flags for slew function // flags for slew function
typedef struct{ typedef struct{
uint32_t slewNguide : 1; // ==1 to gude after slewing uint32_t slewNguide : 1; // ==1 to guide after slewing
} slewflags_t; } slewflags_t;
// mount class // mount class
@ -199,12 +208,11 @@ typedef struct{
mcc_errcodes_t (*init)(conf_t *c); // init device mcc_errcodes_t (*init)(conf_t *c); // init device
void (*quit)(); // deinit void (*quit)(); // deinit
mcc_errcodes_t (*getMountData)(mountdata_t *d); // get last data 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 (*slewTo)(const coordpair_t *target, slewflags_t flags);
mcc_errcodes_t (*moveTo)(const double *X, const double *Y); // move to given position ans stop mcc_errcodes_t (*correctTo)(coordval_pair_t *target);
mcc_errcodes_t (*moveTo)(const coordpair_t *target); // move to given position and stop
mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed mcc_errcodes_t (*moveWspeed)(const coordpair_t *target, const coordpair_t *speed); // move with given max speed
mcc_errcodes_t (*setSpeed)(const double *X, const double *Y); // set speed mcc_errcodes_t (*setSpeed)(const coordpair_t *tagspeed); // set speed
mcc_errcodes_t (*stop)(); // stop mcc_errcodes_t (*stop)(); // stop
mcc_errcodes_t (*emergStop)(); // emergency stop mcc_errcodes_t (*emergStop)(); // emergency stop
mcc_errcodes_t (*shortCmd)(short_command_t *cmd); // send/get short command mcc_errcodes_t (*shortCmd)(short_command_t *cmd); // send/get short command

48
Auxiliary_utils/LibSidServo/ssii.c
View File

@ -35,6 +35,28 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
return checksum; 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 * @brief SSconvstat - convert stat from SSII format to human
* @param s (i) - just read data * @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->motXposition.val = X_MOT2RAD(s->Xmot);
m->motYposition.val = Y_MOT2RAD(s->Ymot); m->motYposition.val = Y_MOT2RAD(s->Ymot);
ChkStopped(s, m);
m->motXposition.t = m->motYposition.t = t; m->motXposition.t = m->motYposition.t = t;
// fill encoder data from here, as there's no separate enc thread // fill encoder data from here, as there's no separate enc thread
if(!Conf.SepEncoder){ if(!Conf.SepEncoder){
@ -154,3 +177,28 @@ int SSstop(int emerg){
return TRUE; 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;
}

34
Auxiliary_utils/LibSidServo/ssii.h
View File

@ -168,6 +168,9 @@
// Loop freq // Loop freq
#define SITECH_LOOP_FREQUENCY (1953.) #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) // steps per revolution (SSI - x4 - for SSI)
// 13312000 / 4 = 3328000 // 13312000 / 4 = 3328000
#define X_MOT_STEPSPERREV_SSI (13312000.) #define X_MOT_STEPSPERREV_SSI (13312000.)
@ -178,13 +181,22 @@
//#define Y_MOT_STEPSPERREV (4394960.) //#define Y_MOT_STEPSPERREV (4394960.)
#define Y_MOT_STEPSPERREV (4394667.) #define Y_MOT_STEPSPERREV (4394667.)
// maximal speeds in rad/s: 10deg/s by X and 8deg/s by Y // convert angle in radians to +-pi
#define X_SPEED_MAX (0.17453) static inline double ang2half(double ang){
#define Y_SPEED_MAX (0.13963) 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 // motor position to radians and back
#define X_MOT2RAD(n) (2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV) #define X_MOT2RAD(n) ang2half(2. * M_PI * ((double)(n)) / X_MOT_STEPSPERREV)
#define Y_MOT2RAD(n) (2. * M_PI * ((double)(n)) / Y_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 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 Y_RAD2MOT(r) ((int32_t)((r) / (2. * M_PI) * Y_MOT_STEPSPERREV))
// motor speed in rad/s and back // motor speed in rad/s and back
@ -206,11 +218,14 @@
#define X_ENC_STEPSPERREV (67108864.) #define X_ENC_STEPSPERREV (67108864.)
#define Y_ENC_STEPSPERREV (67108864.) #define Y_ENC_STEPSPERREV (67108864.)
// encoder zero position // encoder zero position
#define X_ENC_ZERO (46033555) #define X_ENC_ZERO (61245239)
#define Y_ENC_ZERO (36674010) #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 // encoder position to radians and back
#define X_ENC2RAD(n) (2.*M_PI * ((double)(n-X_ENC_ZERO)) / X_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) (2.*M_PI * ((double)(n-Y_ENC_ZERO)) / Y_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 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 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 SSsetterI(const char *cmd, int32_t ival);
int SSstop(int emerg); int SSstop(int emerg);
int SSshortCmd(SSscmd *cmd); int SSshortCmd(SSscmd *cmd);
mcc_errcodes_t updateMotorPos();