Subversion Repositories f9daq

#include "MIKRO.h"
#include <rs232.h>
#ifdef  _CVI_
#  include <userint.h>
#  include <utility.h>
#  include <formatio.h>
#  include <ansi_c.h>
#endif  _CVI_

// #define DEBUG
// #define MIKRO_MAIN

#ifdef MIKRO_MAIN
#  include "MIKRO_uic.h"
#  include "MIKRO_ui.h"
#endif MIKRO_MAIN

#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];

#ifdef _CVI_
int _VI_FUNC MIKRO_Open (int port) {
  int status;

  MIKRO_Port=port;
  status=OpenComConfig (MIKRO_Port, "", 38400, 0, 8, 1, 512, 512);
  if (status) return status;
  SetXMode (MIKRO_Port, 0);
  SetCTSMode (MIKRO_Port, LWRS_HWHANDSHAKE_OFF);
  SetComTime (MIKRO_Port, COMWAIT);
  return 0;
}
# else _CVI_
int _VI_FUNC MIKRO_Open (char *dev) {
  MIKRO_Port=OpenComConfig (dev, "", 38400, 0, 8, 1, 512, 512);
  return 0;
}
#endif _CVI_
void _VI_FUNC MIKRO_Close (void) {
  CloseCom (MIKRO_Port);
}

int _VI_FUNC MIKRO_Cmd (int node, char *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==3) return (0);
  }
  return (-1);
}

int _VI_FUNC MIKRO_Set (int node, char cmd[], int 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==3) return (0);
  }
  return (-1);
}

int _VI_FUNC 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:
          break;
      }
    } else {
      printf("Node %d Communication error => bytes received=0x%02x\n",node,nin);
    }
  }
  return (-1);
}

int _VI_FUNC 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_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");
      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");
      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");
      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");
  Delay(COMDELAY);
  nout = sprintf (MIKRO_Send, "%1d rn\r", 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;
  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
  if (nin!=0) nin--;
  MIKRO_Receive[nin]=0;
  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);
//  printf("MIKRO_MoveTo <-\n");
  return 0;
}

int _VI_FUNC MIKRO_GetPosition (int node, int pos[]) {
  MIKRO_Get(node,"pos",pos);
  return 0;
}

#ifdef MIKRO_MAIN

int __stdcall WinMain (HINSTANCE hInstance, HINSTANCE hPrevInstance,
                       LPSTR lpszCmdLine, int nCmdShow) {
  int i,j;
  int nlines,atline;
  int evcontrl,evpanel,p1_h,ierr,type;
  int rstat,n1,n2, n3, nid;
  int comled;
  int lid[16]= {P1_L1,P1_L2,P1_L3,P1_L4,P1_L5,P1_L6,P1_L7,P1_L8,
                P1_L9,P1_L10,P1_L11,P1_L12,P1_L13,P1_L14,P1_L15,P1_L16
               };
  short int ns2, port;
  char tline[80];

  if (InitCVIRTE (hInstance, 0, 0) == 0) return -1; /* out of memory */

#ifdef _CVI_DEBUG_
  p1_h = LoadPanel (0,"MIKRO_ui.uir", P1);
# else
  p1_h = BuildP1 (0);
#endif /* _CVI_DEBUG_ */
 
  ierr = DisplayPanel (p1_h);
  ierr = SetActiveCtrl (p1_h,P1_B1);
  ierr = SetActiveCtrl (p1_h,P1_B2);
  ierr = SetActiveCtrl (p1_h,P1_B3);

  while (1) {
    ierr = GetUserEvent (1, &evpanel, &evcontrl);
    if (evcontrl == P1_B1) break;
    GetCtrlVal (p1_h, P1_COMLED, &comled);
    if (!comled) {
      GetCtrlVal (p1_h, P1_PORT, &port);
      if (MIKRO_Open (port)) continue;
      MIKRO_Init(1,0);
      MIKRO_Init(2,0);
      MIKRO_Init(3,0);
      SetCtrlVal (p1_h, P1_COMLED, 1);
    }
    switch (evcontrl) {
      case P1_BL:
        MIKRO_Set(1,"lr", -1000);
        MIKRO_Cmd(1,"mv");
        break;
      case P1_BR:
        MIKRO_Set(1,"lr", 1000);
        MIKRO_Cmd(1,"mv");
        break;
      case P1_BU:
        MIKRO_Set(2,"lr", 1000);
        MIKRO_Cmd(2,"mv");
        break;
      case P1_BD:
        MIKRO_Set(2,"lr", -1000);
        MIKRO_Cmd(2,"mv");
        break;
      case P1_BF:
        MIKRO_Set(3,"lr", 1000);
        MIKRO_Cmd(3,"mv");
        break;
      case P1_BB:
        MIKRO_Set(3,"lr", -1000);
        MIKRO_Cmd(3,"mv");
        break;
      case P1_HO:
        ierr = GetCtrlVal (p1_h, P1_N3, &nid);
        MIKRO_ReferenceMove(nid);
        break;
      case P1_GX:
        GetCtrlVal (p1_h, P1_XG, &n2);
        MIKRO_MoveTo(1,n2);
        break;
      case P1_GY:
        GetCtrlVal (p1_h, P1_YG, &n2);
        MIKRO_MoveTo(2,n2);
        break;
      case P1_GZ:
        GetCtrlVal (p1_h, P1_ZG, &n3);
        MIKRO_MoveTo(3,n3);
        break;
      case P1_G:
        GetCtrlVal (p1_h, P1_XG, &n2);
        MIKRO_MoveTo(1,n2);
        GetCtrlVal (p1_h, P1_YG, &n2);
        MIKRO_MoveTo(2,n2);
        GetCtrlVal (p1_h, P1_ZG, &n3);
        MIKRO_MoveTo(3,n3);
      case P1_B2:
      case P1_S1:
        ierr = GetCtrlVal (p1_h, P1_S1, MIKRO_Send);
        nout = strlen (MIKRO_Send);
        MIKRO_Send[nout++]=13;
        rstat = FlushInQ (MIKRO_Port);
        rstat = 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;
        switch (nin) {
          case 0:
            ierr = SetCtrlVal (p1_h, P1_N2, 0);
            break;
          case 9:
            nout = sscanf (MIKRO_Receive, "%x %hx", &n1,&ns2);
            ierr = SetCtrlVal (p1_h, P1_N2, ns2);
            break;
          case 13:
            nout = sscanf (MIKRO_Receive, "%x %x", &n1,&n2);
            ierr = SetCtrlVal (p1_h, P1_N2, n2);
            break;
          default:
            break;
        }
        ierr = SetCtrlVal (p1_h, P1_N1, nin);
        ierr = SetCtrlVal (p1_h, P1_S2, MIKRO_Receive);
        break;
      case P1_B3: // reset
        ierr = GetCtrlVal (p1_h, P1_N3, &nid);
        ierr = GetCtrlVal (p1_h, P1_STAGETYPE, &type);
        MIKRO_Reset(nid);
        rstat = GetNumTextBoxLines (p1_h, P1_T1, &atline);
        rstat = InsertTextBoxLine (p1_h, P1_T1, atline, MIKRO_Receive);
        MIKRO_Init(nid, type);
        break;
      case P1_B4: // status
        ierr = GetCtrlVal (p1_h, P1_N3, &nid);
        n2=MIKRO_GetStat(nid);
        ierr = SetCtrlVal (p1_h, P1_N2, n2);
        ierr = SetCtrlVal (p1_h, P1_N1, nin);
        ierr = SetCtrlVal (p1_h, P1_S2, MIKRO_Receive);
        for (i=0; i<16; i++) {
          ierr = SetCtrlVal (p1_h, lid[i], n2 & 1);
          n2>>=1;
        }
        break;
      case P1_EN:
        ierr = GetCtrlVal (p1_h, P1_N3, &nid);
        MIKRO_Cmd(nid,"en");
        break;
      default:
        break;
    }
    Delay(0.1);
    MIKRO_GetPosition(1,&n2);
    SetCtrlVal (p1_h, P1_XP, n2);
    MIKRO_GetPosition(2,&n2);
    SetCtrlVal (p1_h, P1_YP, n2);
    MIKRO_GetPosition(3,&n2);
    SetCtrlVal (p1_h, P1_ZP, n2);
  }

  GetCtrlVal (p1_h, P1_COMLED, &comled);
  if (comled) MIKRO_Close ();

  return 0;
}
#endif