0,0 → 1,454 |
#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 |