renaming

src/triggerSync/triggerSync.c

@@ -0,0 +1,127 @@
+// >>>>>>> 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 <signal.h>
+#include <unistd.h>
+#include <sys/mman.h>
+#include <native/task.h>
+#include <native/timer.h>
+
+
+extern struct SHM *pshm;
+
+#define CLOCK_RES 1e-9 //Clock resolution is 1 ns by default
+#define LOOP_PERIOD 1e7 //Expressed in ticks
+//RTIME period = 10000000;
+
+RT_TASK trigsync_task;
+void trigsync_func(void *arg)
+{
+  RT_TASK *curtask;
+  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);
+
+  //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;
+
+  tstart = rt_timer_read();
+  pshm = GetSharedMemPtr();
+  srvStart=srvCnt=pshm->ServoCount;
+
+  //Start the task loop
+  while(ctr<200){
+    printf("sLoop count: %d, Loop time: %.5f ms\n", ctr, (rt_timer_read() - tstart)/1000000.0);
+    ctr++;
+    rt_task_wait_period(NULL);
+    //int rt_task_sleep_until
+  }
+  /*  float ang,pos;
+    int i;
+    float srvStart,srvCnt,diff,maxDiff,tmp;
+
+    srvStart=srvCnt=pshm->ServoCount;
+    for(maxDiff=0,i=0;i<10000000;i++)
+    {
+      tmp=pshm->ServoCount;
+      diff=tmp-srvCnt;
+      if(diff>maxDiff)
+        maxDiff=diff;
+      srvCnt=tmp;
+    }
+    printf("srvCnt %d diff %f maxDiff %f\n",(int)(srvCnt-srvStart),diff, maxDiff);*/
+}
+
+void trigsync_run()
+{
+  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);
+}
+
+
+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();
+
+    printf("P:%g\n",pshm->P[1011]);
+    printf("pshm->MaxRtPlc:%d\n",pshm->MaxRtPlc);
+
+    for(i=0;i<8;i++) //MAX_MOTORS instead of 8
+      printf("pshm->Motor[%d].Ctrl:%p\n",i,pshm->Motor[i].Ctrl);
+
+    for(i=0;i<8;i++) //MAX_MOTORS instead of 8
+      printf("pshm->UserAlgo.ServoCtrlAddr[%d]:%d\n",i,pshm->UserAlgo.ServoCtrlAddr[i]);
+
+    for(i=0;i<MAX_PLC;i++) //MAX_MOTORS instead of 8
+      printf("pshm->UserAlgo.BgCplcPID[%d]:%d\n",i,pshm->UserAlgo.BgCplcPID[i]);
+
+    printf("pshm->UserAlgo.RtiCplcPID:%d\n",pshm->UserAlgo.RtiCplcPID);
+    printf("pshm->UserAlgo.RtiCplcMaxTime:%d\n",pshm->UserAlgo.RtiCplcMaxTime);
+    printf("pshm->UserAlgo.CaptCompFuncAddr:%p\n",pshm->UserAlgo.CaptCompFuncAddr);
+
+    trigsync_run();
+    //trigsync_func(0);
+    //loop_task_run();
+
+
+    CloseLibrary();
+    return !err;
+
+    if (initialized)
+        CloseLibrary();
+
+    return 0;
+
+}