wip
This commit is contained in:
@@ -1,9 +1,9 @@
|
||||
// >>>>>>> https://www.ashwinnarayan.com/post/xenomai-realtime-programming-part-2/
|
||||
// >>>>>>> https://xenomai.org/documentation/xenomai-2.4/html/api/group__task.html
|
||||
|
||||
#include <stdio.h>
|
||||
#include <gplib.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <gplib.h>
|
||||
#include <math.h>
|
||||
#include <signal.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
@@ -11,12 +11,18 @@
|
||||
#include <native/timer.h>
|
||||
|
||||
|
||||
extern struct SHM *pshm;
|
||||
extern struct SHM *pshm;
|
||||
|
||||
#define CLOCK_RES 1e-9 //Clock resolution is 1 ns by default
|
||||
#define LOOP_PERIOD 1e7 //Expressed in ticks
|
||||
#define LOOP_PERIOD 1e4 //Expressed in ticks
|
||||
//RTIME period = 10000000;
|
||||
|
||||
//#define FLAG0 (gate3_1->Chan[0].Status&0x8000000)
|
||||
//#define FLAG1 (gate3_1->Chan[1].Status&0x8000000)
|
||||
#define FLAG0 (pshm->P[0])
|
||||
#define FLAG1 (pshm->P[1])
|
||||
|
||||
|
||||
RT_TASK trigsync_task;
|
||||
void trigsync_func(void *arg)
|
||||
{
|
||||
@@ -24,34 +30,73 @@ void trigsync_func(void *arg)
|
||||
RT_TASK_INFO curtaskinfo;
|
||||
int iret = 0;
|
||||
|
||||
RTIME tstart, now;
|
||||
|
||||
curtask = rt_task_self();
|
||||
rt_task_inquire(curtask, &curtaskinfo);
|
||||
|
||||
//Print the info
|
||||
printf("Starting task %s with period of 10 ms ....\n", curtaskinfo.name);
|
||||
printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,LOOP_PERIOD/1000000.f);
|
||||
|
||||
//Make the task periodic with a specified loop period
|
||||
rt_task_set_periodic(NULL, TM_NOW, LOOP_PERIOD);
|
||||
|
||||
int ctr = 0;
|
||||
unsigned srvStart,srvCnt,diff,maxDiff;
|
||||
int i;
|
||||
RTIME rtStart,rtLast,rtCur;
|
||||
unsigned scStart,scLast,diff,maxDiff;
|
||||
struct GateArray3* gate3_1=GetGate3MemPtr(1);
|
||||
|
||||
tstart = rt_timer_read();
|
||||
pshm = GetSharedMemPtr();
|
||||
srvStart=srvCnt=pshm->ServoCount;
|
||||
pshm = GetSharedMemPtr();
|
||||
|
||||
|
||||
/*
|
||||
//Start the task loop
|
||||
while(ctr<200){
|
||||
printf("sLoop count: %d, Loop time: %.5f ms\n", ctr, (rt_timer_read() - tstart)/1000000.0);
|
||||
ctr++;
|
||||
rtStart = rt_timer_read();
|
||||
for(i=0;i<20000;i++)
|
||||
{
|
||||
if (i%10==0)
|
||||
printf("Loop count: %d, Loop time: %.5f ms\n", i, (rt_timer_read() - tStart)/1000000.0);
|
||||
rt_task_wait_period(NULL);
|
||||
//int rt_task_sleep_until
|
||||
}
|
||||
/* float ang,pos;
|
||||
*/
|
||||
|
||||
rtStart=rt_timer_read();
|
||||
printf("Wait for trigger:\n");
|
||||
pshm->Coord[1].Q[0]=-10;
|
||||
while(!FLAG0)
|
||||
rt_task_wait_period(NULL);
|
||||
printf("Flag 0: %.5f ms\n", (rt_timer_read() - rtStart)/1000000.0);
|
||||
|
||||
pshm->Coord[1].Q[0]=-9;
|
||||
if(!pshm->Gather.Enable)
|
||||
pshm->Gather.Enable=2; //start record at flag0' trigger
|
||||
while(!FLAG1)
|
||||
rt_task_wait_period(NULL);
|
||||
//pshm->Gather.Enable=2; //start record at flag1' trigger
|
||||
printf("Flag 1: %.5f ms\n", (rt_timer_read() - rtStart)/1000000.0);
|
||||
rtStart=rtLast=rt_timer_read();
|
||||
scStart=scLast=pshm->ServoCount;
|
||||
pshm->Coord[1].Q[0]=1;
|
||||
pshm->Coord[1].DesTimeBase=pshm->ServoPeriod; //start motion at default speed
|
||||
while(FLAG1)
|
||||
rt_task_wait_period(NULL);
|
||||
|
||||
while(pshm->Gather.Enable)
|
||||
for(i=0;pshm->Gather.Enable;i++)
|
||||
{
|
||||
while(!FLAG1)
|
||||
rt_task_wait_period(NULL);
|
||||
rtLast = rt_timer_read();
|
||||
scLast=pshm->ServoCount;
|
||||
pshm->Coord[1].Q[0]++;
|
||||
while(FLAG1)
|
||||
rt_task_wait_period(NULL);
|
||||
printf("Trigger count: %d, time: %.5f ms ServoCount %d\n", i, (rtLast - rtStart)/1000000.0,scLast-scStart);
|
||||
}
|
||||
|
||||
/* float ang,pos;
|
||||
int i;
|
||||
float srvStart,srvCnt,diff,maxDiff,tmp;
|
||||
float srvStart,srvCnt,diff,maxDiff,tmp;
|
||||
|
||||
srvStart=srvCnt=pshm->ServoCount;
|
||||
for(maxDiff=0,i=0;i<10000000;i++)
|
||||
@@ -65,35 +110,95 @@ void trigsync_func(void *arg)
|
||||
printf("srvCnt %d diff %f maxDiff %f\n",(int)(srvCnt-srvStart),diff, maxDiff);*/
|
||||
}
|
||||
|
||||
void trigsync_run()
|
||||
RT_TASK trigsim_task;
|
||||
void trigsim_func(void *arg)
|
||||
{
|
||||
char str[20];
|
||||
//Lock the memory to avoid memory swapping for this program
|
||||
mlockall(MCL_CURRENT | MCL_FUTURE);
|
||||
printf("Starting cyclic task...\n");
|
||||
//Create the real time task
|
||||
sprintf(str, "cyclic_task");
|
||||
rt_task_create(&trigsync_task, str, 0, 50, T_JOINABLE);
|
||||
//Since task starts in suspended mode, start task
|
||||
rt_task_start(&trigsync_task, &trigsync_func, 0);
|
||||
//Wait for Ctrl-C
|
||||
printf("Wait for Ctrl-C\n");
|
||||
rt_task_join (&trigsync_task);
|
||||
RT_TASK *curtask;
|
||||
RT_TASK_INFO curtaskinfo;
|
||||
int iret = 0;
|
||||
curtask = rt_task_self();
|
||||
rt_task_inquire(curtask, &curtaskinfo);
|
||||
//Print the info
|
||||
printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,LOOP_PERIOD/1000000.f);
|
||||
//Make the task periodic with a specified loop period
|
||||
rt_task_set_periodic(NULL, TM_NOW, LOOP_PERIOD);
|
||||
int i;
|
||||
RTIME rtStart;
|
||||
pshm = GetSharedMemPtr();
|
||||
|
||||
rtStart=rt_timer_read();
|
||||
rt_task_sleep(1000*1000*1000); //in ns
|
||||
pshm->P[0]=1;
|
||||
rt_task_sleep(100*1000*1000); //in ns
|
||||
pshm->P[1]=1;
|
||||
|
||||
for(i=0;i<100&&pshm->Gather.Enable;i++)
|
||||
{
|
||||
pshm->P[1]=1;
|
||||
rt_task_sleep(1*1000*1000); //in ns
|
||||
pshm->P[1]=0;
|
||||
rt_task_sleep(39*1000*1000); //in ns
|
||||
putchar('.');fflush(stdout);
|
||||
}
|
||||
if(pshm->Gather.Enable)
|
||||
{
|
||||
pshm->Gather.Enable=0;
|
||||
}
|
||||
pshm->P[1]=1;
|
||||
rt_task_sleep(1*1000*1000); //in ns
|
||||
pshm->P[1]=0;
|
||||
printf("trigsim_func done:\n");
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
int err;
|
||||
|
||||
|
||||
void trigsync_run()
|
||||
{
|
||||
const char* strSync="trigsync_task";
|
||||
const char* strSim="trigsim_task";
|
||||
pshm->P[0]=0;
|
||||
pshm->P[1]=0;
|
||||
pshm->Coord[1].Q[0]=0;
|
||||
//if(!pshm->Gather.Enable)
|
||||
// pshm->Gather.Enable=1;
|
||||
|
||||
//Lock the memory to avoid memory swapping for this program
|
||||
mlockall(MCL_CURRENT | MCL_FUTURE);
|
||||
printf("Starting rt task...\n");
|
||||
//Create the real time task
|
||||
rt_task_create(&trigsync_task, strSync, 0, 50, T_JOINABLE);
|
||||
//Since task starts in suspended mode, start task
|
||||
rt_task_start(&trigsync_task, &trigsync_func, 0);
|
||||
|
||||
rt_task_create(&trigsim_task, strSim, 0, 50, T_JOINABLE);
|
||||
rt_task_start(&trigsim_task, &trigsim_func, 0);
|
||||
|
||||
printf("Wait end of rt_task\n");
|
||||
|
||||
rt_task_join (&trigsim_task);
|
||||
printf("trigsim_task done\n");
|
||||
|
||||
rt_task_join (&trigsync_task);
|
||||
printf("trigsync_task done\n");
|
||||
|
||||
printf("rt_task ended\n");
|
||||
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
int err;
|
||||
int initialized=0;
|
||||
char s[256];
|
||||
int i;
|
||||
|
||||
if ((err = InitLibrary()) != 0) {
|
||||
abort();
|
||||
}
|
||||
initialized = 1;
|
||||
pshm = GetSharedMemPtr();
|
||||
|
||||
if ((err = InitLibrary()) != 0) {
|
||||
abort();
|
||||
}
|
||||
initialized = 1;
|
||||
pshm = GetSharedMemPtr();
|
||||
|
||||
printf("P:%g\n",pshm->P[1011]);
|
||||
printf("pshm->MaxRtPlc:%d\n",pshm->MaxRtPlc);
|
||||
@@ -116,12 +221,12 @@ int main(int argc, char *argv[])
|
||||
//loop_task_run();
|
||||
|
||||
|
||||
CloseLibrary();
|
||||
return !err;
|
||||
|
||||
if (initialized)
|
||||
CloseLibrary();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
CloseLibrary();
|
||||
return !err;
|
||||
|
||||
if (initialized)
|
||||
CloseLibrary();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
@@ -2,14 +2,14 @@
|
||||
<CodeBlocks_layout_file>
|
||||
<FileVersion major="1" minor="0" />
|
||||
<ActiveTarget name="Debug" />
|
||||
<File name="Makefile" open="1" top="1" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="853" topLine="12" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="triggerSync.c" open="1" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="835" topLine="9" />
|
||||
<Cursor1 position="952" topLine="9" />
|
||||
</Cursor>
|
||||
</File>
|
||||
<File name="Makefile" open="1" top="1" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
|
||||
<Cursor>
|
||||
<Cursor1 position="692" topLine="1" />
|
||||
</Cursor>
|
||||
</File>
|
||||
</CodeBlocks_layout_file>
|
||||
|
||||
Reference in New Issue
Block a user