Subversion Repositories f9daq

Rev

Rev 120 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
120 f9daq 1
#include "MIKRO.h"
2
#include <rs232.h>
3
#ifdef  _CVI_
4
#  include <userint.h>
5
#  include <utility.h>
6
#  include <formatio.h>
7
#  include <ansi_c.h>
8
#endif  _CVI_
9
 
10
// #define DEBUG
11
// #define MIKRO_MAIN
12
 
13
#ifdef MIKRO_MAIN
14
#  include "MIKRO_uic.h"
15
#  include "MIKRO_ui.h"
16
#endif MIKRO_MAIN
17
 
18
#define COMWAIT 0.5
19
#define COMDELAY 0.1
20
 
21
static char MIKRO_Send[100], MIKRO_Receive[100];
22
static char MIKRO_Device, MIKRO_Axes, MIKRO_Response;
23
static int MIKRO_Port;
24
static int nin, nout, rstat;
25
static int MIKRO_type[100];
26
 
27
#ifdef _CVI_
28
int _VI_FUNC MIKRO_Open (int port) {
29
  int status;
30
 
31
  MIKRO_Port=port;
32
  status=OpenComConfig (MIKRO_Port, "", 38400, 0, 8, 1, 512, 512);
33
  if (status) return status;
34
  SetXMode (MIKRO_Port, 0);
35
  SetCTSMode (MIKRO_Port, LWRS_HWHANDSHAKE_OFF);
36
  SetComTime (MIKRO_Port, COMWAIT);
37
  return 0;
38
}
39
# else _CVI_
40
int _VI_FUNC MIKRO_Open (char *dev) {
41
  MIKRO_Port=OpenComConfig (dev, "", 38400, 0, 8, 1, 512, 512);
42
  return 0;
43
}
44
#endif _CVI_
45
void _VI_FUNC MIKRO_Close (void) {
46
  CloseCom (MIKRO_Port);
47
}
48
 
49
int _VI_FUNC MIKRO_Cmd (int node, char *cmd) {
50
  Delay(COMDELAY);
51
  FlushInQ (MIKRO_Port);
52
  nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);
53
  ComWrt (MIKRO_Port, MIKRO_Send, nout);
54
  if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))>0) {
55
    nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
56
    if (nin==3) return (0);
57
  }
58
  return (-1);
59
}
60
 
61
int _VI_FUNC MIKRO_Set (int node, char cmd[], int val) {
62
  Delay(COMDELAY);
63
  FlushInQ (MIKRO_Port);
64
  nout = sprintf (MIKRO_Send, "%1d %s %d\r", node, cmd, val);
65
  ComWrt (MIKRO_Port, MIKRO_Send, nout);
66
  if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))>0) {
67
    nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
68
    if (nin==3) return (0);
69
  }
70
  return (-1);
71
}
72
 
73
int _VI_FUNC MIKRO_Get (int node, char cmd[], int *val) {
74
  short int stmp;
75
 
76
  Delay(COMDELAY);
77
  FlushInQ (MIKRO_Port);
78
  nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd);
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) {
83
      MIKRO_Receive[--nin]=0;
84
      switch (nin) {
85
        case 9:
86
          sscanf (MIKRO_Receive, "%*x %hx",&stmp);
87
          *val=stmp;
88
          return (0);
89
        case 13:
90
          sscanf (MIKRO_Receive, "%*x %x",val);
91
          return (0);
92
        default:
93
          break;
94
      }
95
    } else {
96
      printf("Node %d Communication error => bytes received=0x%02x\n",node,nin);
97
    }
98
  }
99
  return (-1);
100
}
101
 
102
int _VI_FUNC MIKRO_GetStat (int node) {
103
  int tmp;
104
 
105
  Delay(COMDELAY);
106
  FlushInQ (MIKRO_Port);
107
  nout = sprintf (MIKRO_Send, "%1d st\r", node);
108
  ComWrt (MIKRO_Port, MIKRO_Send, nout);
109
  if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))>0) {
110
    nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
111
    if (nin>0) nin--;
112
    MIKRO_Receive[nin]=0;
113
    if (nin==9) {
114
      tmp=0;
115
      sscanf (MIKRO_Receive, "%*x %hx",(short int *)&tmp);
116
      return (tmp);
117
    }
118
  }
119
  return (-1);
120
}
121
 
122
int _VI_FUNC MIKRO_Init (int node, int type) {
123
  MIKRO_type[node]=type;
124
  Delay(0.1);
125
  MIKRO_Cmd(node,"ok 1");
126
  MIKRO_Cmd(node,"ab");
127
  switch (type) {
128
    case 1:// 3M Linear
129
      MIKRO_Cmd(node,"k 1");
130
      MIKRO_Cmd(node,"ad 200");
131
      MIKRO_Cmd(node,"aa 2");
132
      MIKRO_Cmd(node,"fa 1");
133
      MIKRO_Cmd(node,"fd 3000");// Set Max Dynamic Following Error (1000)
134
      MIKRO_Cmd(node,"sr 1000");
135
      MIKRO_Cmd(node,"sp 750");
136
      MIKRO_Cmd(node,"ac 100");
137
      MIKRO_Cmd(node,"dc 200");
138
      MIKRO_Cmd(node,"por 28000");
139
      MIKRO_Cmd(node,"i 600");
140
      MIKRO_Cmd(node,"ano 2350");
141
      MIKRO_Cmd(node,"ls 1");
142
      MIKRO_Cmd(node,"hp 1");
143
      MIKRO_Cmd(node,"hf 1");
144
      break;
145
    case 2:// 3M Rotary
146
      MIKRO_Cmd(node,"k 1");
147
      MIKRO_Cmd(node,"ad 200");
148
      MIKRO_Cmd(node,"aa 1");
149
      MIKRO_Cmd(node,"fa 1");
150
      MIKRO_Cmd(node,"fd 3000");
151
      MIKRO_Cmd(node,"sr 1000");
152
      MIKRO_Cmd(node,"sp 550");
153
      MIKRO_Cmd(node,"ac 100");
154
      MIKRO_Cmd(node,"dc 200");
155
      MIKRO_Cmd(node,"por 28000");
156
      MIKRO_Cmd(node,"i 600");
157
      MIKRO_Cmd(node,"ano 2350");
158
      MIKRO_Cmd(node,"ls 99");
159
      MIKRO_Cmd(node,"hp 1");
160
      MIKRO_Cmd(node,"hf 1");
161
      break;
162
    case 3:// 4M Linear
163
      MIKRO_Cmd(node,"k 1");
164
      MIKRO_Cmd(node,"ad 1000");
165
      MIKRO_Cmd(node,"aa 2");
166
      MIKRO_Cmd(node,"fa 1");
167
      MIKRO_Cmd(node,"fd 3000");
168
      MIKRO_Cmd(node,"sr 1000");
169
      MIKRO_Cmd(node,"sp 1000");
170
      MIKRO_Cmd(node,"ac 100");
171
      MIKRO_Cmd(node,"dc 200");
172
      MIKRO_Cmd(node,"por 28000");
173
      MIKRO_Cmd(node,"i 600");
174
      MIKRO_Cmd(node,"ano 2600");
175
      MIKRO_Cmd(node,"ls 1");
176
      MIKRO_Cmd(node,"hp 1");
177
      MIKRO_Cmd(node,"hf 1");
178
      break;
179
    case 4:// 4M Rotary
180
      MIKRO_Cmd(node,"k 1");
181
      MIKRO_Cmd(node,"ad 100");
182
      MIKRO_Cmd(node,"aa 1");
183
      MIKRO_Cmd(node,"fa 1");
184
      MIKRO_Cmd(node,"fd 3000");
185
      MIKRO_Cmd(node,"sp 800");
186
      MIKRO_Cmd(node,"sr 1000");
187
      MIKRO_Cmd(node,"ac 100");
188
      MIKRO_Cmd(node,"dc 200");
189
      MIKRO_Cmd(node,"por 28000");
190
      MIKRO_Cmd(node,"i 600");
191
      MIKRO_Cmd(node,"ano 2600");
192
      MIKRO_Cmd(node,"ls 99");
193
      break;
194
    default:
195
      break;
196
  }
197
  MIKRO_Cmd(node,"rd 0");
198
  MIKRO_Cmd(node,"n 2");
199
  MIKRO_Cmd(node,"en");
200
  if (type != 0) {
201
    MIKRO_Cmd(node,"eeboot 1");
202
    MIKRO_Cmd(node,"eepsav 1");
203
    Delay(0.1);
204
  }
205
  return 0;
206
}
207
 
208
int _VI_FUNC MIKRO_Reset (int node) {
209
  MIKRO_Cmd(node,"di");
210
  Delay(COMDELAY);
211
  nout = sprintf (MIKRO_Send, "%1d rn\r", node);
212
  ComWrt (MIKRO_Port, MIKRO_Send, nout);
213
 
214
  SetComTime (MIKRO_Port, 20);
215
  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
216
  SetComTime (MIKRO_Port, COMWAIT);
217
  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
218
  if (nin!=0) nin--;
219
  MIKRO_Receive[nin]=0;
220
  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
221
  nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
222
  if (nin!=0) nin--;
223
  MIKRO_Receive[nin]=0;
224
  MIKRO_Init(node,0);
225
  return 0;
226
}
227
 
228
int _VI_FUNC MIKRO_ReferenceMove (int node) {
229
  int fac=10;
230
  int n2,as;
231
 
232
  MIKRO_Cmd(node,"ab");
233
  MIKRO_Cmd(node,"en");
234
  MIKRO_Set(node,"ll",-1000000);
235
  MIKRO_Set(node,"ll",1000000);
236
 
237
  if (!(MIKRO_GetStat(node)&0x8000)) {
238
    MIKRO_Set(node,"v", -100*fac);
239
    do {
240
      MIKRO_GetPosition(node,&n2);
241
      MIKRO_Get(node,"as",&as);
242
      printf("Approaching N-limit node=%d  pos=%d speed=%d\n",node,n2,as);
243
    } while (MIKRO_GetStat(node)&0x1 );
244
    if (!(MIKRO_GetStat(node)&0x8000)) {
245
      printf("N-limit not reached! Trying with half speed.\n");
246
      MIKRO_Set(node,"v", -50*fac);
247
      do {
248
        MIKRO_GetPosition(node,&n2);
249
        MIKRO_Get(node,"as",&as);
250
        printf("Approaching N-limit node=%d  pos=%d speed=%d\n",node,n2,as);
251
      } while (MIKRO_GetStat(node)&0x1 );
252
      if (!(MIKRO_GetStat(node)&0x8000)) {
253
        printf("N-limit not reached! Aborting ...\n");
254
        MIKRO_Cmd(node,"ab");
255
        return -1;
256
      }
257
    }
258
  }
259
 
260
  MIKRO_MoveFor(node,1000);
261
  MIKRO_Set(node,"v", -10*fac);
262
  do {
263
    MIKRO_GetPosition(node,&n2);
264
    MIKRO_Get(node,"as",&as);
265
    printf("Fine tuning 0: node=%d  pos=%d speed=%d\n",node,n2, as);
266
  } while (MIKRO_GetStat(node)&0x1);
267
  if (!(MIKRO_GetStat(node)&0x8000)) {
268
    printf("N-limit not reached! Aborting ...\n");
269
    MIKRO_Cmd(node,"ab");
270
    return -1;
271
  }
272
 
273
  MIKRO_MoveFor(node,1000);
274
  MIKRO_Set(node,"ho",0);
275
  MIKRO_Set(node,"ll",-100);
276
  MIKRO_Set(node,"ll",500100);
277
  return 0;
278
}
279
 
280
int _VI_FUNC MIKRO_MoveFor (int node, int dist) {
281
  MIKRO_Set(node,"lr", dist);
282
  MIKRO_Cmd(node,"mv");
283
  while (MIKRO_GetStat(node)&1) Delay(0.1);
284
  return 0;
285
}
286
 
287
int _VI_FUNC MIKRO_MoveTo (int node, int dest) {
288
//  printf("-> MIKRO_MoveTo \n");
289
  MIKRO_Set(node,"la", dest);
290
  MIKRO_Cmd(node,"mv");
291
  while (MIKRO_GetStat(node)&1) Delay(0.1);
292
//  printf("MIKRO_MoveTo <-\n");
293
  return 0;
294
}
295
 
296
int _VI_FUNC MIKRO_GetPosition (int node, int pos[]) {
297
  MIKRO_Get(node,"pos",pos);
298
  return 0;
299
}
300
 
301
#ifdef MIKRO_MAIN
302
 
303
int __stdcall WinMain (HINSTANCE hInstance, HINSTANCE hPrevInstance,
304
                       LPSTR lpszCmdLine, int nCmdShow) {
305
  int i,j;
306
  int nlines,atline;
307
  int evcontrl,evpanel,p1_h,ierr,type;
308
  int rstat,n1,n2, n3, nid;
309
  int comled;
310
  int lid[16]= {P1_L1,P1_L2,P1_L3,P1_L4,P1_L5,P1_L6,P1_L7,P1_L8,
311
                P1_L9,P1_L10,P1_L11,P1_L12,P1_L13,P1_L14,P1_L15,P1_L16
312
               };
313
  short int ns2, port;
314
  char tline[80];
315
 
316
  if (InitCVIRTE (hInstance, 0, 0) == 0) return -1; /* out of memory */
317
 
133 f9daq 318
#ifdef _CVI_DEBUG_
319
  p1_h = LoadPanel (0,"MIKRO_ui.uir", P1);
320
# else
120 f9daq 321
  p1_h = BuildP1 (0);
133 f9daq 322
#endif /* _CVI_DEBUG_ */
323
 
120 f9daq 324
  ierr = DisplayPanel (p1_h);
325
  ierr = SetActiveCtrl (p1_h,P1_B1);
326
  ierr = SetActiveCtrl (p1_h,P1_B2);
327
  ierr = SetActiveCtrl (p1_h,P1_B3);
328
 
329
  while (1) {
330
    ierr = GetUserEvent (1, &evpanel, &evcontrl);
331
    if (evcontrl == P1_B1) break;
332
    GetCtrlVal (p1_h, P1_COMLED, &comled);
333
    if (!comled) {
334
      GetCtrlVal (p1_h, P1_PORT, &port);
335
      if (MIKRO_Open (port)) continue;
336
      MIKRO_Init(1,0);
337
      MIKRO_Init(2,0);
338
      MIKRO_Init(3,0);
339
      SetCtrlVal (p1_h, P1_COMLED, 1);
340
    }
341
    switch (evcontrl) {
342
      case P1_BL:
343
        MIKRO_Set(1,"lr", -1000);
344
        MIKRO_Cmd(1,"mv");
345
        break;
346
      case P1_BR:
347
        MIKRO_Set(1,"lr", 1000);
348
        MIKRO_Cmd(1,"mv");
349
        break;
350
      case P1_BU:
351
        MIKRO_Set(2,"lr", 1000);
352
        MIKRO_Cmd(2,"mv");
353
        break;
354
      case P1_BD:
355
        MIKRO_Set(2,"lr", -1000);
356
        MIKRO_Cmd(2,"mv");
357
        break;
358
      case P1_BF:
359
        MIKRO_Set(3,"lr", 1000);
360
        MIKRO_Cmd(3,"mv");
361
        break;
362
      case P1_BB:
363
        MIKRO_Set(3,"lr", -1000);
364
        MIKRO_Cmd(3,"mv");
365
        break;
366
      case P1_HO:
367
        ierr = GetCtrlVal (p1_h, P1_N3, &nid);
368
        MIKRO_ReferenceMove(nid);
369
        break;
370
      case P1_GX:
371
        GetCtrlVal (p1_h, P1_XG, &n2);
372
        MIKRO_MoveTo(1,n2);
373
        break;
374
      case P1_GY:
375
        GetCtrlVal (p1_h, P1_YG, &n2);
376
        MIKRO_MoveTo(2,n2);
377
        break;
378
      case P1_GZ:
379
        GetCtrlVal (p1_h, P1_ZG, &n3);
380
        MIKRO_MoveTo(3,n3);
381
        break;
382
      case P1_G:
383
        GetCtrlVal (p1_h, P1_XG, &n2);
384
        MIKRO_MoveTo(1,n2);
385
        GetCtrlVal (p1_h, P1_YG, &n2);
386
        MIKRO_MoveTo(2,n2);
387
        GetCtrlVal (p1_h, P1_ZG, &n3);
388
        MIKRO_MoveTo(3,n3);
389
      case P1_B2:
390
      case P1_S1:
391
        ierr = GetCtrlVal (p1_h, P1_S1, MIKRO_Send);
392
        nout = strlen (MIKRO_Send);
393
        MIKRO_Send[nout++]=13;
394
        rstat = FlushInQ (MIKRO_Port);
395
        rstat = ComWrt (MIKRO_Port, MIKRO_Send, nout);
396
        if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))!=0)
397
          nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa);
398
        if (nin!=0) nin--;
399
        MIKRO_Receive[nin]=0;
400
        switch (nin) {
401
          case 0:
402
            ierr = SetCtrlVal (p1_h, P1_N2, 0);
403
            break;
404
          case 9:
405
            nout = sscanf (MIKRO_Receive, "%x %hx", &n1,&ns2);
406
            ierr = SetCtrlVal (p1_h, P1_N2, ns2);
407
            break;
408
          case 13:
409
            nout = sscanf (MIKRO_Receive, "%x %x", &n1,&n2);
410
            ierr = SetCtrlVal (p1_h, P1_N2, n2);
411
            break;
412
          default:
413
            break;
414
        }
415
        ierr = SetCtrlVal (p1_h, P1_N1, nin);
416
        ierr = SetCtrlVal (p1_h, P1_S2, MIKRO_Receive);
417
        break;
418
      case P1_B3: // reset
419
        ierr = GetCtrlVal (p1_h, P1_N3, &nid);
420
        ierr = GetCtrlVal (p1_h, P1_STAGETYPE, &type);
421
        MIKRO_Reset(nid);
422
        rstat = GetNumTextBoxLines (p1_h, P1_T1, &atline);
423
        rstat = InsertTextBoxLine (p1_h, P1_T1, atline, MIKRO_Receive);
424
        MIKRO_Init(nid, type);
425
        break;
426
      case P1_B4: // status
427
        ierr = GetCtrlVal (p1_h, P1_N3, &nid);
428
        n2=MIKRO_GetStat(nid);
429
        ierr = SetCtrlVal (p1_h, P1_N2, n2);
430
        ierr = SetCtrlVal (p1_h, P1_N1, nin);
431
        ierr = SetCtrlVal (p1_h, P1_S2, MIKRO_Receive);
432
        for (i=0; i<16; i++) {
433
          ierr = SetCtrlVal (p1_h, lid[i], n2 & 1);
434
          n2>>=1;
435
        }
436
        break;
437
      case P1_EN:
438
        ierr = GetCtrlVal (p1_h, P1_N3, &nid);
439
        MIKRO_Cmd(nid,"en");
440
        break;
441
      default:
442
        break;
443
    }
444
    Delay(0.1);
445
    MIKRO_GetPosition(1,&n2);
446
    SetCtrlVal (p1_h, P1_XP, n2);
447
    MIKRO_GetPosition(2,&n2);
448
    SetCtrlVal (p1_h, P1_YP, n2);
449
    MIKRO_GetPosition(3,&n2);
450
    SetCtrlVal (p1_h, P1_ZP, n2);
451
  }
452
 
453
  GetCtrlVal (p1_h, P1_COMLED, &comled);
454
  if (comled) MIKRO_Close ();
455
 
456
  return 0;
457
}
458
#endif