Subversion Repositories f9daq

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
145 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