Subversion Repositories f9daq

Rev

Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

  1. #include "MIKRO.h"
  2. #include <rs232.h>
  3. #ifdef  _CVI_
  4. #  include <userint.h>
  5. #  include <utility.h>
  6. #  include <formatio.h>
  7. #  include <ansi_c.h>
  8. #endif  _CVI_
  9.  
  10. // #define DEBUG
  11. // #define MIKRO_MAIN
  12.  
  13. #ifdef MIKRO_MAIN
  14. #  include "MIKRO_uic.h"
  15. #  include "MIKRO_ui.h"
  16. #endif MIKRO_MAIN
  17.  
  18. #define COMWAIT 0.5
  19. #define COMDELAY 0.1
  20.  
  21. static char MIKRO_Send[100], MIKRO_Receive[100];
  22. static char MIKRO_Device, MIKRO_Axes, MIKRO_Response;
  23. static int MIKRO_Port;
  24. static int nin, nout, rstat;
  25. static int MIKRO_type[100];
  26.  
  27. #ifdef _CVI_
  28. int _VI_FUNC MIKRO_Open (int port) {
  29.   int status;
  30.  
  31.   MIKRO_Port=port;
  32.   status=OpenComConfig (MIKRO_Port, "", 38400, 0, 8, 1, 512, 512);
  33.   if (status) return status;
  34.   SetXMode (MIKRO_Port, 0);
  35.   SetCTSMode (MIKRO_Port, LWRS_HWHANDSHAKE_OFF);
  36.   SetComTime (MIKRO_Port, COMWAIT);
  37.   return 0;
  38. }
  39. # else _CVI_
  40. int _VI_FUNC MIKRO_Open (char *dev) {
  41.   MIKRO_Port=OpenComConfig (dev, "", 38400, 0, 8, 1, 512, 512);
  42.   return 0;
  43. }
  44. #endif _CVI_
  45. void _VI_FUNC MIKRO_Close (void) {
  46.   CloseCom (MIKRO_Port);
  47. }
  48.  
  49. int _VI_FUNC MIKRO_Cmd (int node, char *cmd) {
  50.   Delay(COMDELAY);
  51.   FlushInQ (MIKRO_Port);
  52.   nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);
  53.   ComWrt (MIKRO_Port, MIKRO_Send, nout);
  54.   if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))>0) {
  55.     nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  56.     if (nin==3) return (0);
  57.   }
  58.   return (-1);
  59. }
  60.  
  61. int _VI_FUNC MIKRO_Set (int node, char cmd[], int val) {
  62.   Delay(COMDELAY);
  63.   FlushInQ (MIKRO_Port);
  64.   nout = sprintf (MIKRO_Send, "%1d %s %d\r", node, cmd, val);
  65.   ComWrt (MIKRO_Port, MIKRO_Send, nout);
  66.   if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))>0) {
  67.     nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  68.     if (nin==3) return (0);
  69.   }
  70.   return (-1);
  71. }
  72.  
  73. int _VI_FUNC MIKRO_Get (int node, char cmd[], int *val) {
  74.   short int stmp;
  75.  
  76.   Delay(COMDELAY);
  77.   FlushInQ (MIKRO_Port);
  78.   nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);
  79.   ComWrt (MIKRO_Port, MIKRO_Send, nout);
  80.   if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))>0) {
  81.     nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  82.     if (nin>0) {
  83.       MIKRO_Receive[--nin]=0;
  84.       switch (nin) {
  85.         case 9:
  86.           sscanf (MIKRO_Receive, "%*x %hx",&stmp);
  87.           *val=stmp;
  88.           return (0);
  89.         case 13:
  90.           sscanf (MIKRO_Receive, "%*x %x",val);
  91.           return (0);
  92.         default:
  93.           break;
  94.       }
  95.     } else {
  96.       printf("Node %d Communication error => bytes received=0x%02x\n",node,nin);
  97.     }
  98.   }
  99.   return (-1);
  100. }
  101.  
  102. int _VI_FUNC MIKRO_GetStat (int node) {
  103.   int tmp;
  104.  
  105.   Delay(COMDELAY);
  106.   FlushInQ (MIKRO_Port);
  107.   nout = sprintf (MIKRO_Send, "%1d st\r", node);
  108.   ComWrt (MIKRO_Port, MIKRO_Send, nout);
  109.   if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))>0) {
  110.     nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  111.     if (nin>0) nin--;
  112.     MIKRO_Receive[nin]=0;
  113.     if (nin==9) {
  114.       tmp=0;
  115.       sscanf (MIKRO_Receive, "%*x %hx",(short int *)&tmp);
  116.       return (tmp);
  117.     }
  118.   }
  119.   return (-1);
  120. }
  121.  
  122. int _VI_FUNC MIKRO_Init (int node, int type) {
  123.   MIKRO_type[node]=type;
  124.   Delay(0.1);
  125.   MIKRO_Cmd(node,"ok 1");
  126.   MIKRO_Cmd(node,"ab");
  127.   switch (type) {
  128.     case 1:// 3M Linear
  129.       MIKRO_Cmd(node,"k 1");
  130.       MIKRO_Cmd(node,"ad 200");
  131.       MIKRO_Cmd(node,"aa 2");
  132.       MIKRO_Cmd(node,"fa 1");
  133.       MIKRO_Cmd(node,"fd 3000");// Set Max Dynamic Following Error (1000)
  134.       MIKRO_Cmd(node,"sr 1000");
  135.       MIKRO_Cmd(node,"sp 750");
  136.       MIKRO_Cmd(node,"ac 100");
  137.       MIKRO_Cmd(node,"dc 200");
  138.       MIKRO_Cmd(node,"por 28000");
  139.       MIKRO_Cmd(node,"i 600");
  140.       MIKRO_Cmd(node,"ano 2350");
  141.       MIKRO_Cmd(node,"ls 1");
  142.       MIKRO_Cmd(node,"hp 1");
  143.       MIKRO_Cmd(node,"hf 1");
  144.       break;
  145.     case 2:// 3M Rotary
  146.       MIKRO_Cmd(node,"k 1");
  147.       MIKRO_Cmd(node,"ad 200");
  148.       MIKRO_Cmd(node,"aa 1");
  149.       MIKRO_Cmd(node,"fa 1");
  150.       MIKRO_Cmd(node,"fd 3000");
  151.       MIKRO_Cmd(node,"sr 1000");
  152.       MIKRO_Cmd(node,"sp 550");
  153.       MIKRO_Cmd(node,"ac 100");
  154.       MIKRO_Cmd(node,"dc 200");
  155.       MIKRO_Cmd(node,"por 28000");
  156.       MIKRO_Cmd(node,"i 600");
  157.       MIKRO_Cmd(node,"ano 2350");
  158.       MIKRO_Cmd(node,"ls 99");
  159.       MIKRO_Cmd(node,"hp 1");
  160.       MIKRO_Cmd(node,"hf 1");
  161.       break;
  162.     case 3:// 4M Linear
  163.       MIKRO_Cmd(node,"k 1");
  164.       MIKRO_Cmd(node,"ad 1000");
  165.       MIKRO_Cmd(node,"aa 2");
  166.       MIKRO_Cmd(node,"fa 1");
  167.       MIKRO_Cmd(node,"fd 3000");
  168.       MIKRO_Cmd(node,"sr 1000");
  169.       MIKRO_Cmd(node,"sp 1000");
  170.       MIKRO_Cmd(node,"ac 100");
  171.       MIKRO_Cmd(node,"dc 200");
  172.       MIKRO_Cmd(node,"por 28000");
  173.       MIKRO_Cmd(node,"i 600");
  174.       MIKRO_Cmd(node,"ano 2600");
  175.       MIKRO_Cmd(node,"ls 1");
  176.       MIKRO_Cmd(node,"hp 1");
  177.       MIKRO_Cmd(node,"hf 1");
  178.       break;
  179.     case 4:// 4M Rotary
  180.       MIKRO_Cmd(node,"k 1");
  181.       MIKRO_Cmd(node,"ad 100");
  182.       MIKRO_Cmd(node,"aa 1");
  183.       MIKRO_Cmd(node,"fa 1");
  184.       MIKRO_Cmd(node,"fd 3000");
  185.       MIKRO_Cmd(node,"sp 800");
  186.       MIKRO_Cmd(node,"sr 1000");
  187.       MIKRO_Cmd(node,"ac 100");
  188.       MIKRO_Cmd(node,"dc 200");
  189.       MIKRO_Cmd(node,"por 28000");
  190.       MIKRO_Cmd(node,"i 600");
  191.       MIKRO_Cmd(node,"ano 2600");
  192.       MIKRO_Cmd(node,"ls 99");
  193.       break;
  194.     default:
  195.       break;
  196.   }
  197.   MIKRO_Cmd(node,"rd 0");
  198.   MIKRO_Cmd(node,"n 2");
  199.   MIKRO_Cmd(node,"en");
  200.   if (type != 0) {
  201.     MIKRO_Cmd(node,"eeboot 1");
  202.     MIKRO_Cmd(node,"eepsav 1");
  203.     Delay(0.1);
  204.   }
  205.   return 0;
  206. }
  207.  
  208. int _VI_FUNC MIKRO_Reset (int node) {
  209.   MIKRO_Cmd(node,"di");
  210.   Delay(COMDELAY);
  211.   nout = sprintf (MIKRO_Send, "%1d rn\r", node);
  212.   ComWrt (MIKRO_Port, MIKRO_Send, nout);
  213.  
  214.   SetComTime (MIKRO_Port, 20);
  215.   nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  216.   SetComTime (MIKRO_Port, COMWAIT);
  217.   nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  218.   if (nin!=0) nin--;
  219.   MIKRO_Receive[nin]=0;
  220.   nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  221.   nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  222.   if (nin!=0) nin--;
  223.   MIKRO_Receive[nin]=0;
  224.   MIKRO_Init(node,0);
  225.   return 0;
  226. }
  227.  
  228. int _VI_FUNC MIKRO_ReferenceMove (int node) {
  229.   int fac=10;
  230.   int n2,as;
  231.  
  232.   MIKRO_Cmd(node,"ab");
  233.   MIKRO_Cmd(node,"en");
  234.   MIKRO_Set(node,"ll",-1000000);
  235.   MIKRO_Set(node,"ll",1000000);
  236.  
  237.   if (!(MIKRO_GetStat(node)&0x8000)) {
  238.     MIKRO_Set(node,"v", -100*fac);
  239.     do {
  240.       MIKRO_GetPosition(node,&n2);
  241.       MIKRO_Get(node,"as",&as);
  242.       printf("Approaching N-limit node=%d  pos=%d speed=%d\n",node,n2,as);
  243.     } while (MIKRO_GetStat(node)&0x1 );
  244.     if (!(MIKRO_GetStat(node)&0x8000)) {
  245.       printf("N-limit not reached! Trying with half speed.\n");
  246.       MIKRO_Set(node,"v", -50*fac);
  247.       do {
  248.         MIKRO_GetPosition(node,&n2);
  249.         MIKRO_Get(node,"as",&as);
  250.         printf("Approaching N-limit node=%d  pos=%d speed=%d\n",node,n2,as);
  251.       } while (MIKRO_GetStat(node)&0x1 );
  252.       if (!(MIKRO_GetStat(node)&0x8000)) {
  253.         printf("N-limit not reached! Aborting ...\n");
  254.         MIKRO_Cmd(node,"ab");
  255.         return -1;
  256.       }
  257.     }
  258.   }
  259.  
  260.   MIKRO_MoveFor(node,1000);
  261.   MIKRO_Set(node,"v", -10*fac);
  262.   do {
  263.     MIKRO_GetPosition(node,&n2);
  264.     MIKRO_Get(node,"as",&as);
  265.     printf("Fine tuning 0: node=%d  pos=%d speed=%d\n",node,n2, as);
  266.   } while (MIKRO_GetStat(node)&0x1);
  267.   if (!(MIKRO_GetStat(node)&0x8000)) {
  268.     printf("N-limit not reached! Aborting ...\n");
  269.     MIKRO_Cmd(node,"ab");
  270.     return -1;
  271.   }
  272.  
  273.   MIKRO_MoveFor(node,1000);
  274.   MIKRO_Set(node,"ho",0);
  275.   MIKRO_Set(node,"ll",-100);
  276.   MIKRO_Set(node,"ll",500100);
  277.   return 0;
  278. }
  279.  
  280. int _VI_FUNC MIKRO_MoveFor (int node, int dist) {
  281.   MIKRO_Set(node,"lr", dist);
  282.   MIKRO_Cmd(node,"mv");
  283.   while (MIKRO_GetStat(node)&1) Delay(0.1);
  284.   return 0;
  285. }
  286.  
  287. int _VI_FUNC MIKRO_MoveTo (int node, int dest) {
  288. //  printf("-> MIKRO_MoveTo \n");
  289.   MIKRO_Set(node,"la", dest);
  290.   MIKRO_Cmd(node,"mv");
  291.   while (MIKRO_GetStat(node)&1) Delay(0.1);
  292. //  printf("MIKRO_MoveTo <-\n");
  293.   return 0;
  294. }
  295.  
  296. int _VI_FUNC MIKRO_GetPosition (int node, int pos[]) {
  297.   MIKRO_Get(node,"pos",pos);
  298.   return 0;
  299. }
  300.  
  301. #ifdef MIKRO_MAIN
  302.  
  303. int __stdcall WinMain (HINSTANCE hInstance, HINSTANCE hPrevInstance,
  304.                        LPSTR lpszCmdLine, int nCmdShow) {
  305.   int i,j;
  306.   int nlines,atline;
  307.   int evcontrl,evpanel,p1_h,ierr,type;
  308.   int rstat,n1,n2, n3, nid;
  309.   int comled;
  310.   int lid[16]= {P1_L1,P1_L2,P1_L3,P1_L4,P1_L5,P1_L6,P1_L7,P1_L8,
  311.                 P1_L9,P1_L10,P1_L11,P1_L12,P1_L13,P1_L14,P1_L15,P1_L16
  312.                };
  313.   short int ns2, port;
  314.   char tline[80];
  315.  
  316.   if (InitCVIRTE (hInstance, 0, 0) == 0) return -1; /* out of memory */
  317.  
  318. //  p1_h = LoadPanel (0,"MIKRO_ui.uir", P1);
  319.   p1_h = BuildP1 (0);
  320.   ierr = DisplayPanel (p1_h);
  321.   ierr = SetActiveCtrl (p1_h,P1_B1);
  322.   ierr = SetActiveCtrl (p1_h,P1_B2);
  323.   ierr = SetActiveCtrl (p1_h,P1_B3);
  324.  
  325.   while (1) {
  326.     ierr = GetUserEvent (1, &evpanel, &evcontrl);
  327.     if (evcontrl == P1_B1) break;
  328.     GetCtrlVal (p1_h, P1_COMLED, &comled);
  329.     if (!comled) {
  330.       GetCtrlVal (p1_h, P1_PORT, &port);
  331.       if (MIKRO_Open (port)) continue;
  332.       MIKRO_Init(1,0);
  333.       MIKRO_Init(2,0);
  334.       MIKRO_Init(3,0);
  335.       SetCtrlVal (p1_h, P1_COMLED, 1);
  336.     }
  337.     switch (evcontrl) {
  338.       case P1_BL:
  339.         MIKRO_Set(1,"lr", -1000);
  340.         MIKRO_Cmd(1,"mv");
  341.         break;
  342.       case P1_BR:
  343.         MIKRO_Set(1,"lr", 1000);
  344.         MIKRO_Cmd(1,"mv");
  345.         break;
  346.       case P1_BU:
  347.         MIKRO_Set(2,"lr", 1000);
  348.         MIKRO_Cmd(2,"mv");
  349.         break;
  350.       case P1_BD:
  351.         MIKRO_Set(2,"lr", -1000);
  352.         MIKRO_Cmd(2,"mv");
  353.         break;
  354.       case P1_BF:
  355.         MIKRO_Set(3,"lr", 1000);
  356.         MIKRO_Cmd(3,"mv");
  357.         break;
  358.       case P1_BB:
  359.         MIKRO_Set(3,"lr", -1000);
  360.         MIKRO_Cmd(3,"mv");
  361.         break;
  362.       case P1_HO:
  363.         ierr = GetCtrlVal (p1_h, P1_N3, &nid);
  364.         MIKRO_ReferenceMove(nid);
  365.         break;
  366.       case P1_GX:
  367.         GetCtrlVal (p1_h, P1_XG, &n2);
  368.         MIKRO_MoveTo(1,n2);
  369.         break;
  370.       case P1_GY:
  371.         GetCtrlVal (p1_h, P1_YG, &n2);
  372.         MIKRO_MoveTo(2,n2);
  373.         break;
  374.       case P1_GZ:
  375.         GetCtrlVal (p1_h, P1_ZG, &n3);
  376.         MIKRO_MoveTo(3,n3);
  377.         break;
  378.       case P1_G:
  379.         GetCtrlVal (p1_h, P1_XG, &n2);
  380.         MIKRO_MoveTo(1,n2);
  381.         GetCtrlVal (p1_h, P1_YG, &n2);
  382.         MIKRO_MoveTo(2,n2);
  383.         GetCtrlVal (p1_h, P1_ZG, &n3);
  384.         MIKRO_MoveTo(3,n3);
  385.       case P1_B2:
  386.       case P1_S1:
  387.         ierr = GetCtrlVal (p1_h, P1_S1, MIKRO_Send);
  388.         nout = strlen (MIKRO_Send);
  389.         MIKRO_Send[nout++]=13;
  390.         rstat = FlushInQ (MIKRO_Port);
  391.         rstat = ComWrt (MIKRO_Port, MIKRO_Send, nout);
  392.         if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))!=0)
  393.           nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  394.         if (nin!=0) nin--;
  395.         MIKRO_Receive[nin]=0;
  396.         switch (nin) {
  397.           case 0:
  398.             ierr = SetCtrlVal (p1_h, P1_N2, 0);
  399.             break;
  400.           case 9:
  401.             nout = sscanf (MIKRO_Receive, "%x %hx", &n1,&ns2);
  402.             ierr = SetCtrlVal (p1_h, P1_N2, ns2);
  403.             break;
  404.           case 13:
  405.             nout = sscanf (MIKRO_Receive, "%x %x", &n1,&n2);
  406.             ierr = SetCtrlVal (p1_h, P1_N2, n2);
  407.             break;
  408.           default:
  409.             break;
  410.         }
  411.         ierr = SetCtrlVal (p1_h, P1_N1, nin);
  412.         ierr = SetCtrlVal (p1_h, P1_S2, MIKRO_Receive);
  413.         break;
  414.       case P1_B3: // reset
  415.         ierr = GetCtrlVal (p1_h, P1_N3, &nid);
  416.         ierr = GetCtrlVal (p1_h, P1_STAGETYPE, &type);
  417.         MIKRO_Reset(nid);
  418.         rstat = GetNumTextBoxLines (p1_h, P1_T1, &atline);
  419.         rstat = InsertTextBoxLine (p1_h, P1_T1, atline, MIKRO_Receive);
  420.         MIKRO_Init(nid, type);
  421.         break;
  422.       case P1_B4: // status
  423.         ierr = GetCtrlVal (p1_h, P1_N3, &nid);
  424.         n2=MIKRO_GetStat(nid);
  425.         ierr = SetCtrlVal (p1_h, P1_N2, n2);
  426.         ierr = SetCtrlVal (p1_h, P1_N1, nin);
  427.         ierr = SetCtrlVal (p1_h, P1_S2, MIKRO_Receive);
  428.         for (i=0; i<16; i++) {
  429.           ierr = SetCtrlVal (p1_h, lid[i], n2 & 1);
  430.           n2>>=1;
  431.         }
  432.         break;
  433.       case P1_EN:
  434.         ierr = GetCtrlVal (p1_h, P1_N3, &nid);
  435.         MIKRO_Cmd(nid,"en");
  436.         break;
  437.       default:
  438.         break;
  439.     }
  440.     Delay(0.1);
  441.     MIKRO_GetPosition(1,&n2);
  442.     SetCtrlVal (p1_h, P1_XP, n2);
  443.     MIKRO_GetPosition(2,&n2);
  444.     SetCtrlVal (p1_h, P1_YP, n2);
  445.     MIKRO_GetPosition(3,&n2);
  446.     SetCtrlVal (p1_h, P1_ZP, n2);
  447.   }
  448.  
  449.   GetCtrlVal (p1_h, P1_COMLED, &comled);
  450.   if (comled) MIKRO_Close ();
  451.  
  452.   return 0;
  453. }
  454. #endif
  455.