timur/LibSidServo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

PID works

Browse Source
This commit is contained in:
Edward V. Emelianov
2026-03-23 17:21:33 +03:00
parent 9e7c732afd
commit 071e7fc189
6 changed files with 85 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
PID.c
View File

@@ -122,7 +122,7 @@ static double getspeed(const coordval_t *tagpos, PIDController_t *pid, axisdata_
DBG("error: %g'', cur speed: %g (deg/s)", error * 180. * 3600. / M_PI, axis->speed.val*180./M_PI);
switch(axis->state){
case AXIS_SLEWING:
if(fe < Conf.MaxPointingErr){
if(fe < Conf.MaxFinePointingErr){
axis->state = AXIS_POINTING;
DBG("--> Pointing");
}else{
@@ -216,6 +216,7 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
setStat(xstate, ystate);
}
coordpair_t endpoint;
#if 0
// allow at least PIDMaxDt moving with target speed
double dv = fabs(tagspeed.X - m.encXspeed.val);
double adder = dv/Xlimits.max.accel * (m.encXspeed.val + dv / 2.) // distanse with changing speed
@@ -227,6 +228,16 @@ mcc_errcodes_t correct2(const coordval_pair_t *target){
+ Conf.PIDMaxDt * tagspeed.Y
+ tagspeed.Y * tagspeed.Y / Ylimits.max.accel / 2.;
endpoint.Y = m.encYposition.val + Ysign * adder;
#endif
// allow 10s moving but not more than 10deg and not less than 1deg
double adder = fabs(tagspeed.X) * 10.;
if(adder > 0.17453) adder = 0.17453;
else if(adder < 0.017453) adder = 0.017453;
endpoint.X = m.encXposition.val + Xsign * adder;
adder = fabs(tagspeed.Y) * 10.;
if(adder > 0.17453) adder = 0.17453;
else if(adder < 0.017453) adder = 0.017453;
endpoint.Y = m.encYposition.val + Ysign * adder;
DBG("TAG speeds: %g/%g (deg/s); TAG pos: %g/%g (deg)", tagspeed.X/M_PI*180., tagspeed.Y/M_PI*180., endpoint.X/M_PI*180., endpoint.Y/M_PI*180.);
return Mount.moveWspeed(&endpoint, &tagspeed);
}