moving model
This commit is contained in:
parent
7363d608ea
commit
89b86f8b7f
@ -18,21 +18,27 @@
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <strings.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "moving.h"
|
||||
|
||||
// errors for states: slewing/pointing/guiding
|
||||
#define MAX_POINTING_ERR (50.)
|
||||
#define MAX_GUIDING_ERR (5.)
|
||||
// timeout to "forget" old data from I sum array; seconds
|
||||
#define PID_I_PERIOD (3.)
|
||||
|
||||
static movemodel_t *model = NULL;
|
||||
static FILE *coordslog = NULL;
|
||||
|
||||
typedef enum{
|
||||
Slewing,
|
||||
Pointing,
|
||||
Guiding
|
||||
} state_t;
|
||||
|
||||
static state_t state = Pointing;
|
||||
static state_t state = Slewing;
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
@ -64,6 +70,9 @@ typedef struct {
|
||||
double kp, ki, kd; // PID gains
|
||||
double prev_error; // Previous error
|
||||
double integral; // Integral term
|
||||
double *pidIarray; // array for Integral
|
||||
size_t pidIarrSize; // it's size
|
||||
size_t curIidx; // and index of current element
|
||||
} PIDController;
|
||||
|
||||
static PIDController pid;
|
||||
@ -86,7 +95,7 @@ static sl_option_t opts[] = {
|
||||
// calculate coordinate target for given time (starting from zero)
|
||||
static double target_coord(double t){
|
||||
if(t > 20. && t < 30.) return target_coord(20.);
|
||||
double pos = 5. + 10. * sin(M_2_PI * t / 10.) + 0.2 * (drand48() - 0.5);
|
||||
double pos = 150. + 10. * sin(M_2_PI * t / 10.) + 0.02 * (drand48() - 0.5);
|
||||
return pos;
|
||||
}
|
||||
|
||||
@ -104,23 +113,73 @@ static void pid_init(PIDController *pid, double kp, double ki, double kd) {
|
||||
pid->kd = fabs(kd);
|
||||
pid->prev_error = 0.;
|
||||
pid->integral = 0.;
|
||||
pid->curIidx = 0;
|
||||
pid->pidIarrSize = PID_I_PERIOD / G.dTcorr;
|
||||
if(pid->pidIarrSize < 2) ERRX("I-array for PID have less than 2 elements");
|
||||
pid->pidIarray = MALLOC(double, pid->pidIarrSize);
|
||||
}
|
||||
|
||||
static void pid_clear(PIDController *pid){
|
||||
if(!pid) return;
|
||||
bzero(pid->pidIarray, sizeof(double) * pid->pidIarrSize);
|
||||
pid->integral = 0.;
|
||||
pid->prev_error = 0.;
|
||||
pid->curIidx = 0;
|
||||
}
|
||||
|
||||
static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
|
||||
double error = targcoord - p->coord, fe = fabs(error);
|
||||
if(state == Pointing){
|
||||
if(fe < MAX_GUIDING_ERR) state = Guiding;
|
||||
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;
|
||||
}else if(fe < G.minerr) return p->speed;
|
||||
pid.integral += error * dt;
|
||||
}
|
||||
break;
|
||||
case Pointing:
|
||||
if(fe < MAX_GUIDING_ERR){
|
||||
pid_clear(&pid);
|
||||
state = Guiding;
|
||||
green("--> Guiding\n");
|
||||
}else if(fe > MAX_POINTING_ERR){
|
||||
red("--> Slewing\n");
|
||||
state = Slewing;
|
||||
return (error > 0.) ? limits.max.speed : -limits.max.speed;
|
||||
}
|
||||
break;
|
||||
case Guiding:
|
||||
if(fe > MAX_GUIDING_ERR){
|
||||
red("--> Pointing\n");
|
||||
state = Pointing;
|
||||
}else if(fe < G.minerr){
|
||||
green("At target\n");
|
||||
//pid_clear(&pid);
|
||||
//return p->speed;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
red("Calculate PID\n");
|
||||
double oldi = pid.pidIarray[pid.curIidx], newi = error * dt;
|
||||
pid.pidIarray[pid.curIidx++] = oldi;
|
||||
if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0;
|
||||
pid.integral += newi - oldi;
|
||||
double derivative = (error - pid.prev_error) / dt;
|
||||
pid.prev_error = error;
|
||||
DBG("I=%g", pid.integral);
|
||||
double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative) / dt / 1000.;
|
||||
DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative);
|
||||
double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative);
|
||||
if(state == Pointing) add /= 3.;
|
||||
else if(state == Guiding) add /= 7.;
|
||||
DBG("ADD = %g; new speed = %g", add, p->speed + add);
|
||||
return p->speed + add;
|
||||
if(state == Guiding) return p->speed + add / dt / 10.;
|
||||
return add / dt;
|
||||
}
|
||||
|
||||
// ./moving -l coords -P.5 -I.05 -D1.5
|
||||
// ./moving -l coords -P1.3 -D1.6
|
||||
|
||||
static void start_model(double Tend){
|
||||
double T = 0., Tcorr = 0.;//, Tlast = 0.;
|
||||
@ -167,7 +226,7 @@ int main(int argc, char **argv){
|
||||
if(!coordslog) ERR("Can't open %s", G.xlog);
|
||||
} else coordslog = stdout;
|
||||
if(G.dTmon <= 0.) ERRX("tmon should be > 0.");
|
||||
if(G.dTcorr <= 0.) ERRX("tcor should be > 0.");
|
||||
if(G.dTcorr <= 0. || G.dTcorr > 1.) ERRX("tcor should be > 0. and < 1.");
|
||||
if(G.Tend <= 0.) ERRX("tend should be > 0.");
|
||||
pid_init(&pid, G.P, G.I, G.D);
|
||||
fprintf(coordslog, "%-9s\t%-10s\t%-10s\t%-10s\t%-10s\t%-10s\n", "time", "target", "curpos", "speed", "accel", "error");
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 15.0.1, 2025-04-14T21:38:19. -->
|
||||
<!-- Written by QtCreator 17.0.0, 2025-07-29T13:32:31. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
@ -13,8 +13,8 @@
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
@ -92,6 +92,7 @@
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="bool" key="HasPerBcDcs">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{91347f2c-5221-46a7-80b1-0a054ca02f79}</value>
|
||||
@ -133,6 +134,41 @@
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">По умолчанию</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Развёртывание</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.ShowReachable">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
|
||||
6
LibSidServo/PID_test/plot_jpg
Executable file
6
LibSidServo/PID_test/plot_jpg
Executable file
@ -0,0 +1,6 @@
|
||||
#!/usr/bin/gnuplot
|
||||
|
||||
set terminal jpeg size 1000,500
|
||||
set output "all.jpg"
|
||||
|
||||
plot for [col in "target curpos speed error"] 'coords' using 1:col with lines title columnheader
|
||||
5
LibSidServo/PID_test/ploterr_jpg
Executable file
5
LibSidServo/PID_test/ploterr_jpg
Executable 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
|
||||
Loading…
x
Reference in New Issue
Block a user