sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

enhance sync part 2

Browse Source
This commit is contained in:
zamofing_t
2022-10-06 13:11:35 +02:00
parent 20c6d690e8
commit 9c6d9df97f
4 changed files with 25 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
src/triggerSync/triggerSync
View File

Binary file not shown.

24
src/triggerSync/triggerSync.c
View File

@@ -76,8 +76,8 @@ void trigsync_func(void *arg)
int i;
RTIME rtStart,rtLast,rtCur,rtDiff; //rt* is real time of the rt-process
unsigned int scStart,scLast,scCur,scDiff; //sc* are servo counts of the motion
float srvPer;
float mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed
double srvPer,srvPer2;
double mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed
unsigned int maxDiff;
struct GateArray3* gate3_1=GetGate3MemPtr(1);
@@ -90,6 +90,7 @@ void trigsync_func(void *arg)
}
//unsigned int lastQ12=0; //Q12 value for further sync
double mvTimer;
double mxActPos,mxDesPos,mxHomePos=pshm->Motor[2].HomePos;
double myActPos,myDesPos,myHomePos=pshm->Motor[1].HomePos;
@@ -98,6 +99,7 @@ void trigsync_func(void *arg)
rt_task_wait_period(NULL);
pshm->Coord[1].Q[0]=-2;
srvPer=pshm->ServoPeriod;
srvPer2=pshm->ServoPeriod;
rtStart=rt_timer_read();
printf("Wait for 'arm' event...\n");
@@ -193,13 +195,27 @@ void trigsync_func(void *arg)
mtDes=i*mtPt2Pt+timeOfs;
srvPer=(mtPt2Pt+mtDes-mtAct)/scDiff;
//srvPer=pshm->ServoPeriod; //default speed
pshm->Coord[1].DesTimeBase=srvPer;
// mvTimer > 0 motion leading FEL -> decrease srvPer
// mvTimer < 0 motion lagging FEL -> increase srvPer
// rtDiff > mtPt2Pt -> increase srvPer
// rtDiff < mtPt2Pt -> decrease srvPer
mvTimer=pshm->Motor[1].MoveTimer; //time in current section
if(mvTimer>mtPt2Pt/2) //lagging
mvTimer-=mtPt2Pt;
//srvPer2*=(rtDiff/1E6-mvTimer)/mtPt2Pt;
srvPer2=pshm->ServoPeriod*(rtDiff/1E6-mvTimer)/mtPt2Pt;
pshm->Coord[1].DesTimeBase=srvPer2;
pshm->Coord[1].Q[0]++;
if(mode&8)
{
mxActPos=pshm->Motor[2].ActPos;mxDesPos=pshm->Motor[2].DesPos;
myActPos=pshm->Motor[1].ActPos;myDesPos=pshm->Motor[1].DesPos;
printf(" X,Y ActPos:(%.5g %.5g) DesPos:(%.5g %.5g) ", mxActPos-mxHomePos, myActPos-myHomePos, mxDesPos-mxHomePos, myDesPos-myHomePos);
//printf(" X,Y ActPos:(%.5g %.5g) DesPos:(%.5g %.5g) ", mxActPos-mxHomePos, myActPos-myHomePos, mxDesPos-mxHomePos, myDesPos-myHomePos);
//printf(" X,Y Nsync:(%d) ", pshm->Coord[1].Nsync);
//printf(" X,Y MoveTimer:(%.5g %.5g) ", pshm->Motor[1].MoveTimer,pshm->Motor[2].MoveTimer);
printf(" Nsync:%d, MoveTimer:(%.5g) srvPer2:%.6f ", pshm->Coord[1].Nsync,mvTimer,srvPer2);
printf("Trigger count:%d, rtDiff:%.3fms scDiff:%d mtAct:%.3f mtDes:%.3f srvPer:%.6f ENCVAL:%u \n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer,ENCVAL);
}
rtLast=rtCur;