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

add PID (not tested yet)

Browse Source
This commit is contained in:
Edward V. Emelianov
2025-08-04 17:43:35 +03:00
parent 219cec6055
commit f661dfad44
10 changed files with 300 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
LibSidServo/ssii.c
View File

@@ -37,18 +37,18 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
}
static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *stat){
static void axestat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
if(*prev == INT32_MAX){
*stat = MNT_STOPPED;
*stat = AXIS_STOPPED;
DBG("START");
}else if(*stat != MNT_STOPPED){
}else if(*stat != AXIS_STOPPED){
if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
*stat = MNT_STOPPED;
*stat = AXIS_STOPPED;
DBG("AXE stopped");
}
}else if(*prev != cur){
DBG("AXE moving");
//*stat = MNT_SLEWING;
//*stat = AXIS_SLEWING;
*nstopped = 0;
}
*prev = cur;
@@ -57,8 +57,8 @@ static void axestat(int32_t *prev, int32_t cur, int *nstopped, mnt_status_t *sta
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->Xstatus);
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystatus);
axestat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
axestat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
}
/**
@@ -190,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.Xstatus == MNT_STOPPED){
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);
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.Xstatus == MNT_STOPPED){
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);
if(!SSsetterI(CMD_MOTYSET, Y_RAD2MOT(md.encYposition.val))){
DBG("Ypos sync failed!");