Subversion Repositories f9daq

Rev

Details | Last modification | View Log | RSS feed

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