Subversion Repositories f9daq

#include "rs232.h"

#include "MIKRO.h"

//#define DEBUG

#define COMWAIT 0.5

#define COMDELAY 0.1

static char MIKRO_Send[100], MIKRO_Receive[100];

static char MIKRO_Device, MIKRO_Axes, MIKRO_Response;

static int MIKRO_Port;

static int nin, nout, rstat;

static int MIKRO_type[100];

int MIKRO_Cmd (int node, char *cmd)

{

  printf("Command: %1d %s\n",node,cmd);

  Delay(COMDELAY);

  FlushInQ (MIKRO_Port);

  nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);

  ComWrt (MIKRO_Port, MIKRO_Send, nout);

  if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {

    nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);

    if (nin==2) return (0);

  }

  return (-1);

}

int MIKRO_Set (int node, char cmd[], int val)

{

  printf("Command: %1d %s %d\n",node,cmd, val);

  Delay(COMDELAY);

  FlushInQ (MIKRO_Port);

  nout = sprintf (MIKRO_Send, "%1d %s %d\r", node, cmd, val);

  ComWrt (MIKRO_Port, MIKRO_Send, nout);

  if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {

    nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);

    if (nin==2) return (0);

  }

  return (-1);

}

int MIKRO_Get (int node, char cmd[], int *val)

{

  short int stmp;

  Delay(COMDELAY);

  FlushInQ (MIKRO_Port);

  nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);

  ComWrt (MIKRO_Port, MIKRO_Send, nout);

  if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {

    nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);

    if (nin>0){

//      MIKRO_Receive[--nin]=0;

      switch (nin) {

        case 9:

          sscanf (MIKRO_Receive, "%*x %hx",&stmp);

          *val=stmp;

          return (0);

        case 13:

          sscanf (MIKRO_Receive, "%*x %x",val);

          return (0);

        default:

          printf("Node %d Com error => bytes rcved=0x%02x buf=%s\n",node,nin,MIKRO_Receive);

          break;

      }

    }

  }

  return (-1);

}

int MIKRO_GetStat (int node)

{

  int tmp;

  Delay(COMDELAY);

  FlushInQ (MIKRO_Port);

  nout = sprintf (MIKRO_Send, "%1d st\r", node);

  ComWrt (MIKRO_Port, MIKRO_Send, nout);

  if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {

    nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);

//    if (nin>0) nin--;

    MIKRO_Receive[nin]=0;

    if (nin==9) {

      tmp=0;

      sscanf (MIKRO_Receive, "%*x %hx",(short int *)&tmp);

          return (tmp);

    }

  }

  return (-1);

}

int _VI_FUNC MIKRO_Open (char * dev)

{

  MIKRO_Port=OpenComConfig (dev, "", 38400, 0, 8, 1, 512, 512);

//  SetXMode (MIKRO_Port, 0);

//  SetCTSMode (MIKRO_Port, LWRS_HWHANDSHAKE_OFF);

//  SetComTime (MIKRO_Port, COMWAIT);

  return 0;

}

int _VI_FUNC MIKRO_Init (int node, int type)

{

 MIKRO_type[node]=type;

 Delay(0.1);

 MIKRO_Cmd(node,"ok 1");  

 MIKRO_Cmd(node,"ab");  

 switch (type){

   case 1:                              // 3M Linear

     MIKRO_Cmd(node,"k 1");

     MIKRO_Cmd(node,"ad 200");

     MIKRO_Cmd(node,"aa 2");

     MIKRO_Cmd(node,"fa 1");

     MIKRO_Cmd(node,"fd 3000");    // Set Max Dynamic Following Error (1000)

     MIKRO_Cmd(node,"sr 1000");

     MIKRO_Cmd(node,"sp 750");

     MIKRO_Cmd(node,"ac 100");

     MIKRO_Cmd(node,"dc 200");

     MIKRO_Cmd(node,"por 28000");

     MIKRO_Cmd(node,"i 600");

     MIKRO_Cmd(node,"ano 2350");

     MIKRO_Cmd(node,"ls 1");

     MIKRO_Cmd(node,"hp 1");

     MIKRO_Cmd(node,"hf 1");

     break;

   case 2:                              // 3M Rotary

     MIKRO_Cmd(node,"k 1");

     MIKRO_Cmd(node,"ad 200");

     MIKRO_Cmd(node,"aa 1");

     MIKRO_Cmd(node,"fa 1");

     MIKRO_Cmd(node,"fd 3000");    // Set Max Dynamic Following Error (1000)

     MIKRO_Cmd(node,"sr 1000");

     MIKRO_Cmd(node,"sp 550");

     MIKRO_Cmd(node,"ac 100");

     MIKRO_Cmd(node,"dc 200");

     MIKRO_Cmd(node,"por 28000");

     MIKRO_Cmd(node,"i 600");

     MIKRO_Cmd(node,"ano 2350");

     MIKRO_Cmd(node,"ls 99");

     MIKRO_Cmd(node,"hp 1");

     MIKRO_Cmd(node,"hf 1");

     break;

   case 3:                              // 4M Linear

     MIKRO_Cmd(node,"k 1");

     MIKRO_Cmd(node,"ad 1000");

     MIKRO_Cmd(node,"aa 2");

     MIKRO_Cmd(node,"fa 1");

     MIKRO_Cmd(node,"fd 3000");    // Set Max Dynamic Following Error (1000)

     MIKRO_Cmd(node,"sr 1000");

     MIKRO_Cmd(node,"sp 1000");

     MIKRO_Cmd(node,"ac 100");

     MIKRO_Cmd(node,"dc 200");

     MIKRO_Cmd(node,"por 28000");

     MIKRO_Cmd(node,"i 600");

     MIKRO_Cmd(node,"ano 2600");

     MIKRO_Cmd(node,"ls 1");

     MIKRO_Cmd(node,"hp 1");

     MIKRO_Cmd(node,"hf 1");

     break;

   case 4:                              // 4M Rotary

     MIKRO_Cmd(node,"k 1");

     MIKRO_Cmd(node,"ad 100");

     MIKRO_Cmd(node,"aa 1");

     MIKRO_Cmd(node,"fa 1");

     MIKRO_Cmd(node,"fd 3000");    // Set Max Dynamic Following Error (1000)

     MIKRO_Cmd(node,"sp 800");

     MIKRO_Cmd(node,"sr 1000");

     MIKRO_Cmd(node,"ac 100");

     MIKRO_Cmd(node,"dc 200");

     MIKRO_Cmd(node,"por 28000");

     MIKRO_Cmd(node,"i 600");

     MIKRO_Cmd(node,"ano 2600");

     MIKRO_Cmd(node,"ls 99");

     break;

   default:

     break;

  }

  MIKRO_Cmd(node,"rd 0");

  MIKRO_Cmd(node,"n 2");

  MIKRO_Cmd(node,"en");

  if (type != 0){

    MIKRO_Cmd(node,"eeboot 1");

    MIKRO_Cmd(node,"eepsav 1");

    Delay(0.1);

  }

  return 0;

}

int _VI_FUNC MIKRO_Reset (int node)

{

  MIKRO_Cmd(node,"di"); // disables the node

  Delay(COMDELAY);

  nout = sprintf (MIKRO_Send, "%1d rn\r", node); // resets the node

  ComWrt (MIKRO_Port, MIKRO_Send, nout);

  SetComTime (MIKRO_Port, 20);

  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);

  SetComTime (MIKRO_Port, COMWAIT);

  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);

//  if (nin!=0) nin--;

//  MIKRO_Receive[nin]=0;

  printf("%s\n",MIKRO_Receive);

  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);

  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);

//  if (nin!=0) nin--;

//  MIKRO_Receive[nin]=0;

  printf("%s\n",MIKRO_Receive);

  MIKRO_Init(node,0);

  return 0;

}

int _VI_FUNC MIKRO_ReferenceMove (int node)

{

  int fac=10;

  int n2,as;

  MIKRO_Cmd(node,"ab");

  MIKRO_Cmd(node,"en");

  MIKRO_Set(node,"ll",-1000000);

  MIKRO_Set(node,"ll",1000000);

  if (!(MIKRO_GetStat(node)&0x8000)){

    MIKRO_Set(node,"v", -100*fac);

    do {

      MIKRO_GetPosition(node,&n2);

      MIKRO_Get(node,"as",&as);  

      printf("Approaching N-limit node=%d  pos=%d speed=%d\n",node,n2,as);

    } while (MIKRO_GetStat(node)&0x1 );

    if (!(MIKRO_GetStat(node)&0x8000)){

      printf("N-limit not reached! Trying with half speed.\n");

      MIKRO_Set(node,"v", -50*fac);

      do {

        MIKRO_GetPosition(node,&n2);

        MIKRO_Get(node,"as",&as);  

        printf("Approaching N-limit node=%d  pos=%d speed=%d\n",node,n2,as);

      } while (MIKRO_GetStat(node)&0x1 );

      if (!(MIKRO_GetStat(node)&0x8000)){

        printf("N-limit not reached! Aborting ...\n");

        MIKRO_Cmd(node,"ab");

        return -1;

      }

    }

  }

  MIKRO_MoveFor(node,1000);

  MIKRO_Set(node,"v", -10*fac);

  do {

    MIKRO_GetPosition(node,&n2);

    MIKRO_Get(node,"as",&as);

    printf("Fine tuning 0: node=%d  pos=%d speed=%d\n",node,n2, as);

  } while (MIKRO_GetStat(node)&0x1);

  if (!(MIKRO_GetStat(node)&0x8000)){

    printf("N-limit not reached! Aborting ...\n");

    MIKRO_Cmd(node,"ab");

    return -1;

  }

  MIKRO_MoveFor(node,1000);

  MIKRO_Set(node,"ho",0);

  MIKRO_Set(node,"ll",-100);

  MIKRO_Set(node,"ll",500100);

  return 0;

}

int _VI_FUNC MIKRO_MoveFor (int node, int dist)

{

  MIKRO_Set(node,"lr", dist);

  MIKRO_Cmd(node,"mv");

  while (MIKRO_GetStat(node)&1) Delay(0.1);

  return 0;

}

int _VI_FUNC MIKRO_MoveTo (int node, int dest)

{

//  printf("-> MIKRO_MoveTo \n");

  MIKRO_Set(node,"la", dest);

  MIKRO_Cmd(node,"mv");

  while (MIKRO_GetStat(node)&1) Delay(0.1);

  return 0;

}

int _VI_FUNC MIKRO_GetPosition (int node, int pos[])

{

  MIKRO_Get(node,"pos",pos);

  return 0;

}

void _VI_FUNC MIKRO_Close (void)

{

CloseCom (MIKRO_Port);

}