Subversion Repositories f9daq

Rev

Blame | Last modification | View Log | RSS feed

#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);
}