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

19
LibSidServo/ssii.c
View File

@@ -36,19 +36,18 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
return checksum;
}
static void axestat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
if(*prev == INT32_MAX){
*stat = AXIS_STOPPED;
DBG("START");
}else if(*stat != AXIS_STOPPED){
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
*stat = AXIS_STOPPED;
DBG("AXE stopped");
DBG("AXIS stopped");
}
}else if(*prev != cur){
DBG("AXE moving");
//*stat = AXIS_SLEWING;
DBG("AXIS moving");
*nstopped = 0;
}
*prev = cur;
@@ -57,8 +56,8 @@ static void axestat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *st
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->Xstate);
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
}
/**
@@ -191,14 +190,14 @@ mcc_errcodes_t updateMotorPos(){
DBG("got; t pos x/y: %g/%g; tnow: %g", md.encXposition.t, md.encYposition.t, t);
mcc_errcodes_t OK = MCC_E_OK;
if(fabs(md.motXposition.val - md.encXposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync X: motors=%g, axes=%g", md.motXposition.val, md.encXposition.val);
DBG("NEED to sync X: motors=%g, axiss=%g", md.motXposition.val, md.encXposition.val);
if(!SSsetterI(CMD_MOTXSET, X_RAD2MOT(md.encXposition.val))){
DBG("Xpos sync failed!");
OK = MCC_E_FAILED;
}else DBG("Xpos sync OK, Dt=%g", nanotime() - t0);
}
if(fabs(md.motYposition.val - md.encYposition.val) > MCC_ENCODERS_ERROR && md.Xstate == AXIS_STOPPED){
DBG("NEED to sync Y: motors=%g, axes=%g", md.motYposition.val, md.encYposition.val);
DBG("NEED to sync Y: motors=%g, axiss=%g", md.motYposition.val, md.encYposition.val);
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
DBG("Ypos sync failed!");
OK = MCC_E_FAILED;