Go to most recent revision |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
#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 */
// p1_h = LoadPanel (0,"MIKRO_ui.uir", P1);
p1_h
= BuildP1
(0);
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