WebSVN - f9daq - Blame - Rev 146 - /lab/sipmscan/trunk/src/MIKRO/MIKRO.c

Rev	Author	Line No.	Line
146	f9daq	1	#include "rs232.h"
		2	#include "MIKRO.h"
		3
		4	//#define DEBUG
		5
		6	#define COMWAIT 0.5
		7	#define COMDELAY 0.1
		8
		9	static char MIKRO_Send[100], MIKRO_Receive[100];
		10	static char MIKRO_Device, MIKRO_Axes, MIKRO_Response;
		11	static int MIKRO_Port;
		12	static int nin, nout, rstat;
		13	static int MIKRO_type[100];
		14
		15	int MIKRO_Cmd (int node, char *cmd)
		16	{
		17	printf("Command: %1d %s\n",node,cmd);
		18	Delay(COMDELAY);
		19	FlushInQ (MIKRO_Port);
		20	nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);
		21	ComWrt (MIKRO_Port, MIKRO_Send, nout);
		22	if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {
		23	nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
		24	if (nin==2) return (0);
		25	}
		26	return (-1);
		27	}
		28
		29	int MIKRO_Set (int node, char cmd[], int val)
		30	{
		31	printf("Command: %1d %s %d\n",node,cmd, val);
		32	Delay(COMDELAY);
		33	FlushInQ (MIKRO_Port);
		34	nout = sprintf (MIKRO_Send, "%1d %s %d\r", node, cmd, val);
		35	ComWrt (MIKRO_Port, MIKRO_Send, nout);
		36	if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {
		37	nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
		38	if (nin==2) return (0);
		39	}
		40	return (-1);
		41	}
		42
		43	int MIKRO_Get (int node, char cmd[], int *val)
		44	{
		45	short int stmp;
		46
		47	Delay(COMDELAY);
		48	FlushInQ (MIKRO_Port);
		49	nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);
		50	ComWrt (MIKRO_Port, MIKRO_Send, nout);
		51	if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {
		52	nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
		53	if (nin>0){
		54	// MIKRO_Receive[--nin]=0;
		55	switch (nin) {
		56	case 9:
		57	sscanf (MIKRO_Receive, "%*x %hx",&stmp);
		58	*val=stmp;
		59	return (0);
		60	case 13:
		61	sscanf (MIKRO_Receive, "%*x %x",val);
		62	return (0);
		63	default:
		64	printf("Node %d Com error => bytes rcved=0x%02x buf=%s\n",node,nin,MIKRO_Receive);
		65	break;
		66	}
		67	}
		68	}
		69	return (-1);
		70	}
		71
		72	int MIKRO_GetStat (int node)
		73	{
		74	int tmp;
		75
		76	Delay(COMDELAY);
		77	FlushInQ (MIKRO_Port);
		78	nout = sprintf (MIKRO_Send, "%1d st\r", node);
		79	ComWrt (MIKRO_Port, MIKRO_Send, nout);
		80	if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) {
		81	nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
		82	// if (nin>0) nin--;
		83	MIKRO_Receive[nin]=0;
		84	if (nin==9) {
		85	tmp=0;
		86	sscanf (MIKRO_Receive, "%x %hx",(short int )&tmp);
		87	return (tmp);
		88	}
		89	}
		90	return (-1);
		91	}
		92
		93	int _VI_FUNC MIKRO_Open (char * dev)
		94	{
		95
		96
		97	MIKRO_Port=OpenComConfig (dev, "", 38400, 0, 8, 1, 512, 512);
		98	// SetXMode (MIKRO_Port, 0);
		99	// SetCTSMode (MIKRO_Port, LWRS_HWHANDSHAKE_OFF);
		100	// SetComTime (MIKRO_Port, COMWAIT);
		101
		102	return 0;
		103	}
		104
		105	int _VI_FUNC MIKRO_Init (int node, int type)
		106	{
		107	MIKRO_type[node]=type;
		108	Delay(0.1);
		109	MIKRO_Cmd(node,"ok 1");
		110	MIKRO_Cmd(node,"ab");
		111	switch (type){
		112	case 1: // 3M Linear
		113	MIKRO_Cmd(node,"k 1");
		114	MIKRO_Cmd(node,"ad 200");
		115	MIKRO_Cmd(node,"aa 2");
		116	MIKRO_Cmd(node,"fa 1");
		117	MIKRO_Cmd(node,"fd 3000"); // Set Max Dynamic Following Error (1000)
		118	MIKRO_Cmd(node,"sr 1000");
		119	MIKRO_Cmd(node,"sp 750");
		120	MIKRO_Cmd(node,"ac 100");
		121	MIKRO_Cmd(node,"dc 200");
		122	MIKRO_Cmd(node,"por 28000");
		123	MIKRO_Cmd(node,"i 600");
		124	MIKRO_Cmd(node,"ano 2350");
		125	MIKRO_Cmd(node,"ls 1");
		126	MIKRO_Cmd(node,"hp 1");
		127	MIKRO_Cmd(node,"hf 1");
		128	break;
		129	case 2: // 3M Rotary
		130	MIKRO_Cmd(node,"k 1");
		131	MIKRO_Cmd(node,"ad 200");
		132	MIKRO_Cmd(node,"aa 1");
		133	MIKRO_Cmd(node,"fa 1");
		134	MIKRO_Cmd(node,"fd 3000"); // Set Max Dynamic Following Error (1000)
		135	MIKRO_Cmd(node,"sr 1000");
		136	MIKRO_Cmd(node,"sp 550");
		137	MIKRO_Cmd(node,"ac 100");
		138	MIKRO_Cmd(node,"dc 200");
		139	MIKRO_Cmd(node,"por 28000");
		140	MIKRO_Cmd(node,"i 600");
		141	MIKRO_Cmd(node,"ano 2350");
		142	MIKRO_Cmd(node,"ls 99");
		143	MIKRO_Cmd(node,"hp 1");
		144	MIKRO_Cmd(node,"hf 1");
		145	break;
		146	case 3: // 4M Linear
		147	MIKRO_Cmd(node,"k 1");
		148	MIKRO_Cmd(node,"ad 1000");
		149	MIKRO_Cmd(node,"aa 2");
		150	MIKRO_Cmd(node,"fa 1");
		151	MIKRO_Cmd(node,"fd 3000"); // Set Max Dynamic Following Error (1000)
		152	MIKRO_Cmd(node,"sr 1000");
		153	MIKRO_Cmd(node,"sp 1000");
		154	MIKRO_Cmd(node,"ac 100");
		155	MIKRO_Cmd(node,"dc 200");
		156	MIKRO_Cmd(node,"por 28000");
		157	MIKRO_Cmd(node,"i 600");
		158	MIKRO_Cmd(node,"ano 2600");
		159	MIKRO_Cmd(node,"ls 1");
		160	MIKRO_Cmd(node,"hp 1");
		161	MIKRO_Cmd(node,"hf 1");
		162	break;
		163	case 4: // 4M Rotary
		164	MIKRO_Cmd(node,"k 1");
		165	MIKRO_Cmd(node,"ad 100");
		166	MIKRO_Cmd(node,"aa 1");
		167	MIKRO_Cmd(node,"fa 1");
		168	MIKRO_Cmd(node,"fd 3000"); // Set Max Dynamic Following Error (1000)
		169	MIKRO_Cmd(node,"sp 800");
		170	MIKRO_Cmd(node,"sr 1000");
		171	MIKRO_Cmd(node,"ac 100");
		172	MIKRO_Cmd(node,"dc 200");
		173	MIKRO_Cmd(node,"por 28000");
		174	MIKRO_Cmd(node,"i 600");
		175	MIKRO_Cmd(node,"ano 2600");
		176	MIKRO_Cmd(node,"ls 99");
		177	break;
		178	default:
		179	break;
		180	}
		181	MIKRO_Cmd(node,"rd 0");
		182	MIKRO_Cmd(node,"n 2");
		183	MIKRO_Cmd(node,"en");
		184	if (type != 0){
		185	MIKRO_Cmd(node,"eeboot 1");
		186	MIKRO_Cmd(node,"eepsav 1");
		187	Delay(0.1);
		188	}
		189	return 0;
		190	}
		191
		192	int _VI_FUNC MIKRO_Reset (int node)
		193	{
		194
		195	MIKRO_Cmd(node,"di"); // disables the node
		196
		197	Delay(COMDELAY);
		198	nout = sprintf (MIKRO_Send, "%1d rn\r", node); // resets the node
		199	ComWrt (MIKRO_Port, MIKRO_Send, nout);
		200
		201	SetComTime (MIKRO_Port, 20);
		202	nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
		203	SetComTime (MIKRO_Port, COMWAIT);
		204	nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
		205	// if (nin!=0) nin--;
		206	// MIKRO_Receive[nin]=0;
		207	printf("%s\n",MIKRO_Receive);
		208	nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
		209	nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
		210	// if (nin!=0) nin--;
		211	// MIKRO_Receive[nin]=0;
		212	printf("%s\n",MIKRO_Receive);
		213	MIKRO_Init(node,0);
		214	return 0;
		215	}
		216
		217	int _VI_FUNC MIKRO_ReferenceMove (int node)
		218	{
		219	int fac=10;
		220	int n2,as;
		221
		222	MIKRO_Cmd(node,"ab");
		223	MIKRO_Cmd(node,"en");
		224	MIKRO_Set(node,"ll",-1000000);
		225	MIKRO_Set(node,"ll",1000000);
		226
		227	if (!(MIKRO_GetStat(node)&0x8000)){
		228	MIKRO_Set(node,"v", -100*fac);
		229	do {
		230	MIKRO_GetPosition(node,&n2);
		231	MIKRO_Get(node,"as",&as);
		232	printf("Approaching N-limit node=%d pos=%d speed=%d\n",node,n2,as);
		233	} while (MIKRO_GetStat(node)&0x1 );
		234	if (!(MIKRO_GetStat(node)&0x8000)){
		235	printf("N-limit not reached! Trying with half speed.\n");
		236	MIKRO_Set(node,"v", -50*fac);
		237	do {
		238	MIKRO_GetPosition(node,&n2);
		239	MIKRO_Get(node,"as",&as);
		240	printf("Approaching N-limit node=%d pos=%d speed=%d\n",node,n2,as);
		241	} while (MIKRO_GetStat(node)&0x1 );
		242	if (!(MIKRO_GetStat(node)&0x8000)){
		243	printf("N-limit not reached! Aborting ...\n");
		244	MIKRO_Cmd(node,"ab");
		245	return -1;
		246	}
		247	}
		248	}
		249	MIKRO_MoveFor(node,1000);
		250	MIKRO_Set(node,"v", -10*fac);
		251	do {
		252	MIKRO_GetPosition(node,&n2);
		253	MIKRO_Get(node,"as",&as);
		254	printf("Fine tuning 0: node=%d pos=%d speed=%d\n",node,n2, as);
		255	} while (MIKRO_GetStat(node)&0x1);
		256	if (!(MIKRO_GetStat(node)&0x8000)){
		257	printf("N-limit not reached! Aborting ...\n");
		258	MIKRO_Cmd(node,"ab");
		259	return -1;
		260	}
		261
		262	MIKRO_MoveFor(node,1000);
		263	MIKRO_Set(node,"ho",0);
		264	MIKRO_Set(node,"ll",-100);
		265	MIKRO_Set(node,"ll",500100);
		266	return 0;
		267	}
		268
		269	int _VI_FUNC MIKRO_MoveFor (int node, int dist)
		270	{
		271	MIKRO_Set(node,"lr", dist);
		272	MIKRO_Cmd(node,"mv");
		273	while (MIKRO_GetStat(node)&1) Delay(0.1);
		274	return 0;
		275	}
		276
		277	int _VI_FUNC MIKRO_MoveTo (int node, int dest)
		278	{
		279	// printf("-> MIKRO_MoveTo \n");
		280	MIKRO_Set(node,"la", dest);
		281	MIKRO_Cmd(node,"mv");
		282	while (MIKRO_GetStat(node)&1) Delay(0.1);
		283	return 0;
		284	}
		285
		286	int _VI_FUNC MIKRO_GetPosition (int node, int pos[])
		287	{
		288	MIKRO_Get(node,"pos",pos);
		289	return 0;
		290	}
		291
		292	void _VI_FUNC MIKRO_Close (void)
		293	{
		294	CloseCom (MIKRO_Port);
		295	}
		296

Subversion Repositories f9daq

(root)/lab/sipmscan/trunk/src/MIKRO/MIKRO.c - Rev 146