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

PID seems like almost working, but I have a great problem with model!

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-08-05 18:55:21 +03:00
parent 864e257884
commit 138e4bf84d
16 changed files with 218 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
LibSidServo/ramp.c
View File

@@ -23,7 +23,12 @@
#include "main.h"
#include "ramp.h"
/*
#ifdef EBUG
#undef DBG
#define DBG(...)
#endif
*/
static double coord_tolerance = COORD_TOLERANCE_DEFAULT;
static void emstop(movemodel_t *m, double _U_ t){
@@ -66,9 +71,9 @@ ret:
* @return FALSE if can't move with given parameters
*/
static int calc(movemodel_t *m, moveparam_t *x, double t){
DBG("coord/speed: %g/%g", x->coord, x->speed);
if(!x) return FALSE;
pthread_mutex_lock(&m->mutex);
DBG("target coord/speed: %g/%g; current coord: %g", x->coord, x->speed, m->curparams.coord);
int ret = FALSE;
if(x->coord < m->Min.coord || x->coord > m->Max.coord){
DBG("Wrong coordinage [%g, %g]", m->Min.coord, m->Max.coord);
@@ -101,6 +106,33 @@ static int calc(movemodel_t *m, moveparam_t *x, double t){
ret = TRUE;
goto ret;
}
#if 0
model_move2 (/home/eddy/C-files/LibSidServo/movingmodel.c, line 63): MOVE to -0.785398 at speed 0.00408621
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 76): target coord/speed: -0.785398/0.00408621; current coord: 0.181616
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 91): Dx=0.967014, sign=-1, dt23=0.0185812, dx23=3.79634e-05
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 101): dt0s=0.0377057, dx0s=0.000156326, curspeed=0.0082919
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 110): SIGN of speed should be changed!
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 0: t=3.9028, coord=0.181616, speed=0.0082919, accel=-0.219911
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 1: t=3.9405, coord=0.181772, speed=0, accel=-0.219911
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 2: t=3.95908, coord=0.181734, speed=-0.00408621, accel=0.219911
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 3: t=3.97766, coord=-0.785398, speed=0, accel=0
model_move2 (/home/eddy/C-files/LibSidServo/movingmodel.c, line 63): MOVE to -0.785398 at speed 0.0361538
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 76): target coord/speed: -0.785398/0.0361538; current coord: 0.177343
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 91): Dx=0.962741, sign=-1, dt23=0.218049, dx23=0.00394164
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 101): dt0s=0.185651, dx0s=0.00285737, curspeed=0.0307821
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 110): SIGN of speed should be changed!
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 0: t=3.9028, coord=0.177343, speed=0.0307821, accel=-0.165806
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 1: t=4.08845, coord=0.1802, speed=0, accel=-0.165806
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 2: t=4.3065, coord=0.176259, speed=-0.0361538, accel=0.165806
calc (/home/eddy/C-files/LibSidServo/ramp.c, line 215): 3: t=4.52455, coord=-0.785398, speed=0, accel=0
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 233): REACHED STOPping stage @ t=3.97837
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[3]=3.97766,
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[2]=3.95908,
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[1]=3.9405,
proc (/home/eddy/C-files/LibSidServo/ramp.c, line 235): T[0]=3.9028,
#endif
if(m->curparams.speed * sign < 0. || m->state == ST_STOP || (dx0s > Dx + coord_tolerance)){ // we should change speed sign
DBG("SIGN of speed should be changed!");
double sign_current = (m->curparams.speed >= 0.) ? 1. : -1.;
@@ -225,6 +257,11 @@ static movestate_t proc(movemodel_t *m, moveparam_t *next, double t){
if(m->movingstage == STAGE_STOPPED){
m->curparams.coord = m->Params[STAGE_STOPPED].coord;
pthread_mutex_unlock(&m->mutex);
DBG("REACHED STOPping stage @ t=%g", t);
for(int s = STAGE_STOPPED; s >= 0; --s){
DBG("T[%d]=%g, ", s, m->Times[s]);
}
fflush(stdout);
emstop(m, t);
goto ret;
}