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

PID test almost in ssii format

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-07-30 13:43:39 +03:00
parent c862d160fe
commit 502014bee4
33 changed files with 1381 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

141
LibSidServo/PID_test/Dramp.c
View File

@@ -1,141 +0,0 @@
/*
* 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,
};

64
LibSidServo/PID_test/PID.c Normal file
View File

@@ -0,0 +1,64 @@
/*
* 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 <stdlib.h>
#include <strings.h>
#include <usefull_macros.h>
#include "PID.h"
PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz){
if(!gain || Iarrsz < 3) return NULL;
PIDController_t *pid = (PIDController_t*)calloc(1, sizeof(PIDController_t));
pid->gain = *gain;
pid->pidIarrSize = Iarrsz;
pid->pidIarray = (double*)calloc(Iarrsz, sizeof(double));
return pid;
}
void pid_clear(PIDController_t *pid){
if(!pid) return;
DBG("CLEAR PID PARAMETERS");
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
pid->integral = 0.;
pid->prev_error = 0.;
pid->curIidx = 0;
}
void pid_delete(PIDController_t **pid){
if(!pid || !*pid) return;
if((*pid)->pidIarray) free((*pid)->pidIarray);
free(*pid);
*pid = NULL;
}
double pid_calculate(PIDController_t *pid, double error, double dt){
// calculate flowing integral
double oldi = pid->pidIarray[pid->curIidx], newi = error * dt;
DBG("oldi/new: %g, %g", oldi, newi);
pid->pidIarray[pid->curIidx++] = newi;
if(pid->curIidx >= pid->pidIarrSize) pid->curIidx = 0;
pid->integral += newi - oldi;
double derivative = (error - pid->prev_error) / dt;
pid->prev_error = error;
double sum = pid->gain.P * error + pid->gain.I * pid->integral + pid->gain.D * derivative;
DBG("P=%g, I=%g, D=%g; sum=%g", pid->gain.P * error, pid->gain.I * pid->integral, pid->gain.D * derivative, sum);
return sum;
}

20
LibSidServo/PID_test/Dramp.h → LibSidServo/PID_test/PID.h
View File

@@ -18,6 +18,22 @@
#pragma once
#include "moving_private.h"
#include <stddef.h>
extern movemodel_t dumb;
typedef struct{
double P, I, D;
} PIDpar_t;
typedef struct {
PIDpar_t gain; // 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_t;
PIDController_t *pid_create(PIDpar_t *gain, size_t Iarrsz);
void pid_clear(PIDController_t *pid);
void pid_delete(PIDController_t **pid);
double pid_calculate(PIDController_t *pid, double error, double dt);

27
LibSidServo/PID_test/Sramp.c
View File

@@ -1,27 +0,0 @@
/*
* 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
LibSidServo/PID_test/Sramp.h
View File

@@ -1,23 +0,0 @@
/*
* 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;

145
LibSidServo/PID_test/main.c
View File

@@ -22,13 +22,18 @@
#include <usefull_macros.h>
#include "moving.h"
#include "PID.h"
// errors for states: slewing/pointing/guiding
#define MAX_POINTING_ERR (50.)
#define MAX_GUIDING_ERR (5.)
// 10-degrees zone - Coordinate-driven PID
#define MAX_POINTING_ERR (36000.)
// 1-arcminute zone - Velocity-dtiven PID
#define MAX_GUIDING_ERR (60.)
// timeout to "forget" old data from I sum array; seconds
#define PID_I_PERIOD (3.)
// PID for coordinate-driven and velocity-driven parts
static PIDController_t *pidC = NULL, *pidV = NULL;
static movemodel_t *model = NULL;
static FILE *coordslog = NULL;
@@ -48,93 +53,64 @@ typedef struct{
double dTcorr;
double Tend;
double minerr;
double P, I, D;
double startcoord;
double error;
PIDpar_t gainC, gainV;
} pars;
static pars G = {
.ramptype = "t",
.dTmon = 0.01,
.dTcorr = 0.05,
.Tend = 100.,
.minerr = 0.1,
.P = 0.8,
.gainC.P = 0.1,
.gainV.P = 0.1,
.startcoord = 100.,
};
static limits_t limits = {
.min = {.coord = -1e6, .speed = 0.01, .accel = 0.1},
.max = {.coord = 1e6, .speed = 1e3, .accel = 500.},
.jerk = 10.
.max = {.coord = 6648000, .speed = 36000., .accel = 36000.}
};
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"},
{"propC", NEED_ARG, NULL, 'P', arg_double, APTR(&G.gainC.P), "P-coefficient of coordinate-driven PID"},
{"integC", NEED_ARG, NULL, 'I', arg_double, APTR(&G.gainC.I), "I-coefficient of coordinate-driven PID"},
{"diffC", NEED_ARG, NULL, 'D', arg_double, APTR(&G.gainC.D), "D-coefficient of coordinate-driven PID"},
{"propV", NEED_ARG, NULL, 'p', arg_double, APTR(&G.gainV.P), "P-coefficient of velocity-driven PID"},
{"integV", NEED_ARG, NULL, 'i', arg_double, APTR(&G.gainV.I), "I-coefficient of velocity-driven PID"},
{"diffV", NEED_ARG, NULL, 'd', arg_double, APTR(&G.gainV.D), "D-coefficient of velocity-driven PID"},
{"xstart", NEED_ARG, NULL, '0', arg_double, APTR(&G.startcoord), "starting coordinate of target"},
{"error", NEED_ARG, NULL, 'E', arg_double, APTR(&G.error), "error range"},
// 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);
if(t > 20. && t < 30.) return 0.;
//double pos = G.startcoord + 15. * t + G.error * (drand48() - 0.5);
double pos = G.startcoord + 15. * sin(2*M_PI * t / 10.) + G.error * (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);
PIDController_t *pid = NULL;
switch(state){
case Slewing:
if(fe < MAX_POINTING_ERR){
pid_clear(&pid);
pid_clear(pidC);
state = Pointing;
green("--> Pointing\n");
pid = pidC;
}else{
red("Slewing...\n");
return (error > 0.) ? limits.max.speed : -limits.max.speed;
@@ -142,54 +118,55 @@ static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
break;
case Pointing:
if(fe < MAX_GUIDING_ERR){
pid_clear(&pid);
pid_clear(pidV);
state = Guiding;
green("--> Guiding\n");
pid = pidV;
}else if(fe > MAX_POINTING_ERR){
red("--> Slewing\n");
state = Slewing;
return (error > 0.) ? limits.max.speed : -limits.max.speed;
}
} else pid = pidC;
break;
case Guiding:
pid= pidV;
if(fe > MAX_GUIDING_ERR){
red("--> Pointing\n");
state = Pointing;
pid_clear(pidC);
pid = pidC;
}else if(fe < G.minerr){
green("At target\n");
//pid_clear(&pid);
//return p->speed;
}
}else printf("Current error: %g\n", fe);
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;
if(!pid){
WARNX("where is PID?"); return p->speed;
}
double tagspeed = pid_calculate(pid, error, dt);
if(state == Guiding) return p->speed + tagspeed;
return tagspeed;
}
// ./moving -l coords -P.5 -I.05 -D1.5
// ./moving -l coords -P1.3 -D1.6
// -P0.8 -D0.1 -I0.02 -p20 -d.5 -i.02
// another: P0.8 -D0.1 -I0.02 -p5 -d0.9 -i0.1
static void start_model(double Tend){
double T = 0., Tcorr = 0.;//, Tlast = 0.;
double T = 0., Tcorr = 0.;
moveparam_t target;
uint64_t N = 0;
double errmax = 0., errsum = 0., errsum2 = 0.;
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(state == Guiding){
double ae = fabs(error);
if(ae > errmax) errmax = ae;
errsum += error; errsum2 += error * error;
++N;
}
if(T - Tcorr >= G.dTcorr){ // check correction
double speed = getNewSpeed(&p, nextcoord, T - Tcorr);
target.coord = (speed > 0) ? p.coord + 5e5 : p.coord - 5e5;
@@ -215,6 +192,9 @@ static void start_model(double Tend){
T, nextcoord, p.coord, p.speed, p.accel, error);
T += G.dTmon;
}
printf("\n\n\n"); red("Calculated errors in `guiding` mode:\n");
double mean = errsum / (double)N;
printf("max error: %g, mean error: %g, std: %g\n\n", errmax, mean, sqrt(errsum2/(double)N - mean*mean));
}
int main(int argc, char **argv){
@@ -228,16 +208,15 @@ int main(int argc, char **argv){
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);
pidC = pid_create(&G.gainC, PID_I_PERIOD / G.dTcorr);
pidV = pid_create(&G.gainV, PID_I_PERIOD / G.dTcorr);
if(!pidC || !pidV) ERRX("Can't init PID regulators");
model = init_moving(&limits);
if(!model) ERRX("Can't init moving model: check parameters");
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
start_model(G.Tend);
pid_delete(&pidC);
pid_delete(&pidV);
fclose(coordslog);
return 0;
}

20
LibSidServo/PID_test/moving.c
View File

@@ -24,12 +24,9 @@
#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;
static movemodel_t *model = &trapez;
double coord_tolerance = COORD_TOLERANCE_DEFAULT;
double time_tick = TIME_TICK_DEFAULT;
@@ -56,21 +53,8 @@ static void chkminmax(double *min, double *max){
*max = t;
}
movemodel_t *init_moving(ramptype_t type, limits_t *l){
movemodel_t *init_moving(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;

10
LibSidServo/PID_test/moving.h
View File

@@ -26,13 +26,6 @@
#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
@@ -48,7 +41,6 @@ typedef struct{ // all values could be both as positive and negative
typedef struct{
moveparam_t min;
moveparam_t max;
double jerk;
} limits_t;
typedef struct{
@@ -64,7 +56,7 @@ typedef struct{
extern double coord_tolerance;
double nanot();
movemodel_t *init_moving(ramptype_t type, limits_t *l);
movemodel_t *init_moving(limits_t *l);
int init_coordtol(double tolerance);
int init_timetick(double tick);
int move_to(moveparam_t *target, double t);

2
LibSidServo/PID_test/moving_model.files
View File

@@ -1,5 +1,7 @@
Dramp.c
Dramp.h
PID.c
PID.h
Sramp.c
Sramp.h
Tramp.c