Subversion Repositories f9daq

Rev

Blame | Last modification | View Log | RSS feed

  1. #include "rs232.h"
  2. #include "MIKRO.h"
  3.  
  4. //#define DEBUG
  5.  
  6. #define COMWAIT 0.5
  7. #define COMDELAY 0.1
  8.  
  9. static char MIKRO_Send[100], MIKRO_Receive[100];
  10. static char MIKRO_Device, MIKRO_Axes, MIKRO_Response;
  11. static int MIKRO_Port;
  12. static int nin, nout, rstat;
  13. static int MIKRO_type[100];
  14.  
  15. int MIKRO_Cmd (int node, char *cmd)
  16. {
  17.   printf("Command: %1d %s\n",node,cmd);
  18.   Delay(COMDELAY);
  19.   FlushInQ (MIKRO_Port);
  20.   nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);
  21.   ComWrt (MIKRO_Port, MIKRO_Send, nout);
  22.   if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {
  23.     nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  24.     if (nin==2) return (0);
  25.   }
  26.   return (-1);
  27. }
  28.  
  29. int MIKRO_Set (int node, char cmd[], int val)
  30. {
  31.   printf("Command: %1d %s %d\n",node,cmd, val);
  32.   Delay(COMDELAY);
  33.   FlushInQ (MIKRO_Port);
  34.   nout = sprintf (MIKRO_Send, "%1d %s %d\r", node, cmd, val);
  35.   ComWrt (MIKRO_Port, MIKRO_Send, nout);
  36.   if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {
  37.     nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  38.     if (nin==2) return (0);
  39.   }
  40.   return (-1);
  41. }
  42.  
  43. int MIKRO_Get (int node, char cmd[], int *val)
  44. {
  45.   short int stmp;
  46.  
  47.   Delay(COMDELAY);
  48.   FlushInQ (MIKRO_Port);
  49.   nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);
  50.   ComWrt (MIKRO_Port, MIKRO_Send, nout);
  51.   if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {
  52.     nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  53.     if (nin>0){
  54. //      MIKRO_Receive[--nin]=0;
  55.       switch (nin) {
  56.         case 9:
  57.           sscanf (MIKRO_Receive, "%*x %hx",&stmp);
  58.           *val=stmp;
  59.           return (0);
  60.         case 13:
  61.           sscanf (MIKRO_Receive, "%*x %x",val);
  62.           return (0);
  63.         default:
  64.           printf("Node %d Com error => bytes rcved=0x%02x buf=%s\n",node,nin,MIKRO_Receive);
  65.           break;
  66.       }
  67.     }
  68.   }
  69.   return (-1);
  70. }
  71.  
  72. int MIKRO_GetStat (int node)
  73. {
  74.   int tmp;
  75.  
  76.   Delay(COMDELAY);
  77.   FlushInQ (MIKRO_Port);
  78.   nout = sprintf (MIKRO_Send, "%1d st\r", node);
  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) nin--;
  83.     MIKRO_Receive[nin]=0;
  84.     if (nin==9) {
  85.       tmp=0;
  86.       sscanf (MIKRO_Receive, "%*x %hx",(short int *)&tmp);
  87.           return (tmp);
  88.     }
  89.   }
  90.   return (-1);
  91. }
  92.  
  93. int _VI_FUNC MIKRO_Open (char * dev)
  94. {
  95.  
  96.  
  97.   MIKRO_Port=OpenComConfig (dev, "", 38400, 0, 8, 1, 512, 512);
  98. //  SetXMode (MIKRO_Port, 0);
  99. //  SetCTSMode (MIKRO_Port, LWRS_HWHANDSHAKE_OFF);
  100. //  SetComTime (MIKRO_Port, COMWAIT);
  101.  
  102.   return 0;
  103. }
  104.  
  105. int _VI_FUNC MIKRO_Init (int node, int type)
  106. {
  107.  MIKRO_type[node]=type;
  108.  Delay(0.1);
  109.  MIKRO_Cmd(node,"ok 1");  
  110.  MIKRO_Cmd(node,"ab");  
  111.  switch (type){
  112.    case 1:                              // 3M Linear
  113.      MIKRO_Cmd(node,"k 1");
  114.      MIKRO_Cmd(node,"ad 200");
  115.      MIKRO_Cmd(node,"aa 2");
  116.      MIKRO_Cmd(node,"fa 1");
  117.      MIKRO_Cmd(node,"fd 3000");    // Set Max Dynamic Following Error (1000)
  118.      MIKRO_Cmd(node,"sr 1000");
  119.      MIKRO_Cmd(node,"sp 750");
  120.      MIKRO_Cmd(node,"ac 100");
  121.      MIKRO_Cmd(node,"dc 200");
  122.      MIKRO_Cmd(node,"por 28000");
  123.      MIKRO_Cmd(node,"i 600");
  124.      MIKRO_Cmd(node,"ano 2350");
  125.      MIKRO_Cmd(node,"ls 1");
  126.      MIKRO_Cmd(node,"hp 1");
  127.      MIKRO_Cmd(node,"hf 1");
  128.      break;
  129.    case 2:                              // 3M Rotary
  130.      MIKRO_Cmd(node,"k 1");
  131.      MIKRO_Cmd(node,"ad 200");
  132.      MIKRO_Cmd(node,"aa 1");
  133.      MIKRO_Cmd(node,"fa 1");
  134.      MIKRO_Cmd(node,"fd 3000");    // Set Max Dynamic Following Error (1000)
  135.      MIKRO_Cmd(node,"sr 1000");
  136.      MIKRO_Cmd(node,"sp 550");
  137.      MIKRO_Cmd(node,"ac 100");
  138.      MIKRO_Cmd(node,"dc 200");
  139.      MIKRO_Cmd(node,"por 28000");
  140.      MIKRO_Cmd(node,"i 600");
  141.      MIKRO_Cmd(node,"ano 2350");
  142.      MIKRO_Cmd(node,"ls 99");
  143.      MIKRO_Cmd(node,"hp 1");
  144.      MIKRO_Cmd(node,"hf 1");
  145.      break;
  146.    case 3:                              // 4M Linear
  147.      MIKRO_Cmd(node,"k 1");
  148.      MIKRO_Cmd(node,"ad 1000");
  149.      MIKRO_Cmd(node,"aa 2");
  150.      MIKRO_Cmd(node,"fa 1");
  151.      MIKRO_Cmd(node,"fd 3000");    // Set Max Dynamic Following Error (1000)
  152.      MIKRO_Cmd(node,"sr 1000");
  153.      MIKRO_Cmd(node,"sp 1000");
  154.      MIKRO_Cmd(node,"ac 100");
  155.      MIKRO_Cmd(node,"dc 200");
  156.      MIKRO_Cmd(node,"por 28000");
  157.      MIKRO_Cmd(node,"i 600");
  158.      MIKRO_Cmd(node,"ano 2600");
  159.      MIKRO_Cmd(node,"ls 1");
  160.      MIKRO_Cmd(node,"hp 1");
  161.      MIKRO_Cmd(node,"hf 1");
  162.      break;
  163.    case 4:                              // 4M Rotary
  164.      MIKRO_Cmd(node,"k 1");
  165.      MIKRO_Cmd(node,"ad 100");
  166.      MIKRO_Cmd(node,"aa 1");
  167.      MIKRO_Cmd(node,"fa 1");
  168.      MIKRO_Cmd(node,"fd 3000");    // Set Max Dynamic Following Error (1000)
  169.      MIKRO_Cmd(node,"sp 800");
  170.      MIKRO_Cmd(node,"sr 1000");
  171.      MIKRO_Cmd(node,"ac 100");
  172.      MIKRO_Cmd(node,"dc 200");
  173.      MIKRO_Cmd(node,"por 28000");
  174.      MIKRO_Cmd(node,"i 600");
  175.      MIKRO_Cmd(node,"ano 2600");
  176.      MIKRO_Cmd(node,"ls 99");
  177.      break;
  178.    default:
  179.      break;
  180.   }
  181.   MIKRO_Cmd(node,"rd 0");
  182.   MIKRO_Cmd(node,"n 2");
  183.   MIKRO_Cmd(node,"en");
  184.   if (type != 0){
  185.     MIKRO_Cmd(node,"eeboot 1");
  186.     MIKRO_Cmd(node,"eepsav 1");
  187.     Delay(0.1);
  188.   }
  189.   return 0;
  190. }
  191.  
  192. int _VI_FUNC MIKRO_Reset (int node)
  193. {
  194.  
  195.   MIKRO_Cmd(node,"di"); // disables the node
  196.  
  197.   Delay(COMDELAY);
  198.   nout = sprintf (MIKRO_Send, "%1d rn\r", node); // resets the node
  199.   ComWrt (MIKRO_Port, MIKRO_Send, nout);
  200.  
  201.   SetComTime (MIKRO_Port, 20);
  202.   nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  203.   SetComTime (MIKRO_Port, COMWAIT);
  204.   nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  205. //  if (nin!=0) nin--;
  206. //  MIKRO_Receive[nin]=0;
  207.   printf("%s\n",MIKRO_Receive);
  208.   nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  209.   nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  210. //  if (nin!=0) nin--;
  211. //  MIKRO_Receive[nin]=0;
  212.   printf("%s\n",MIKRO_Receive);
  213.   MIKRO_Init(node,0);
  214.   return 0;
  215. }
  216.  
  217. int _VI_FUNC MIKRO_ReferenceMove (int node)
  218. {
  219.   int fac=10;
  220.   int n2,as;
  221.  
  222.   MIKRO_Cmd(node,"ab");
  223.   MIKRO_Cmd(node,"en");
  224.   MIKRO_Set(node,"ll",-1000000);
  225.   MIKRO_Set(node,"ll",1000000);
  226.  
  227.   if (!(MIKRO_GetStat(node)&0x8000)){
  228.     MIKRO_Set(node,"v", -100*fac);
  229.     do {
  230.       MIKRO_GetPosition(node,&n2);
  231.       MIKRO_Get(node,"as",&as);  
  232.       printf("Approaching N-limit node=%d  pos=%d speed=%d\n",node,n2,as);
  233.     } while (MIKRO_GetStat(node)&0x1 );
  234.     if (!(MIKRO_GetStat(node)&0x8000)){
  235.       printf("N-limit not reached! Trying with half speed.\n");
  236.       MIKRO_Set(node,"v", -50*fac);
  237.       do {
  238.         MIKRO_GetPosition(node,&n2);
  239.         MIKRO_Get(node,"as",&as);  
  240.         printf("Approaching N-limit node=%d  pos=%d speed=%d\n",node,n2,as);
  241.       } while (MIKRO_GetStat(node)&0x1 );
  242.       if (!(MIKRO_GetStat(node)&0x8000)){
  243.         printf("N-limit not reached! Aborting ...\n");
  244.         MIKRO_Cmd(node,"ab");
  245.         return -1;
  246.       }
  247.     }
  248.   }
  249.   MIKRO_MoveFor(node,1000);
  250.   MIKRO_Set(node,"v", -10*fac);
  251.   do {
  252.     MIKRO_GetPosition(node,&n2);
  253.     MIKRO_Get(node,"as",&as);
  254.     printf("Fine tuning 0: node=%d  pos=%d speed=%d\n",node,n2, as);
  255.   } while (MIKRO_GetStat(node)&0x1);
  256.   if (!(MIKRO_GetStat(node)&0x8000)){
  257.     printf("N-limit not reached! Aborting ...\n");
  258.     MIKRO_Cmd(node,"ab");
  259.     return -1;
  260.   }
  261.  
  262.   MIKRO_MoveFor(node,1000);
  263.   MIKRO_Set(node,"ho",0);
  264.   MIKRO_Set(node,"ll",-100);
  265.   MIKRO_Set(node,"ll",500100);
  266.   return 0;
  267. }
  268.  
  269. int _VI_FUNC MIKRO_MoveFor (int node, int dist)
  270. {
  271.   MIKRO_Set(node,"lr", dist);
  272.   MIKRO_Cmd(node,"mv");
  273.   while (MIKRO_GetStat(node)&1) Delay(0.1);
  274.   return 0;
  275. }
  276.  
  277. int _VI_FUNC MIKRO_MoveTo (int node, int dest)
  278. {
  279. //  printf("-> MIKRO_MoveTo \n");
  280.   MIKRO_Set(node,"la", dest);
  281.   MIKRO_Cmd(node,"mv");
  282.   while (MIKRO_GetStat(node)&1) Delay(0.1);
  283.   return 0;
  284. }
  285.  
  286. int _VI_FUNC MIKRO_GetPosition (int node, int pos[])
  287. {
  288.   MIKRO_Get(node,"pos",pos);
  289.   return 0;
  290. }
  291.  
  292. void _VI_FUNC MIKRO_Close (void)
  293. {
  294. CloseCom (MIKRO_Port);
  295. }
  296.  
  297.