1 | /*-------------------------------------------------------------------------*/
|
---|
2 | /* DIM section for Array definition (arrays are globals) */
|
---|
3 | /*-------------------------------------------------------------------------*/
|
---|
4 |
|
---|
5 | DIM errlist[9] /* idx 1=number of valid entries */
|
---|
6 |
|
---|
7 | /* ----------------------------------------------------------------------- */
|
---|
8 | /* */
|
---|
9 | /* Version: */
|
---|
10 | /* */
|
---|
11 | kVERSION = 0 /* */
|
---|
12 | kSUBVERSION = 62 /* */
|
---|
13 | /* */
|
---|
14 | /* HISTORY: */
|
---|
15 | /* */
|
---|
16 | /* * V0.62: */
|
---|
17 | /* - changed handling of 0x2000/1/2 added /3 */
|
---|
18 | /* */
|
---|
19 | /* * V0.61: */
|
---|
20 | /* - corrected problems with the error handling */
|
---|
21 | /* */
|
---|
22 | /* * V0.60: */
|
---|
23 | /* - introduced syncronisation */
|
---|
24 | /* */
|
---|
25 | /* * V0.52: */
|
---|
26 | /* - changed the handling of the endswitch error (unknown switch) */
|
---|
27 | /* */
|
---|
28 | /* * V0.51: */
|
---|
29 | /* - made errlist working */
|
---|
30 | /* */
|
---|
31 | /* * V0.50: */
|
---|
32 | /* - Rearanged many object numbers */
|
---|
33 | /* - Added object dictonary to comments */
|
---|
34 | /* */
|
---|
35 | /* * V0.43: */
|
---|
36 | /* - Added Object 0x1003, 0x1004, 0x1005 */
|
---|
37 | /* */
|
---|
38 | /* * V0.42: */
|
---|
39 | /* - Added APOS to PDO1 */
|
---|
40 | /* */
|
---|
41 | /* * V0.41: */
|
---|
42 | /* - Period Interrupt diabled while HOME */
|
---|
43 | /* */
|
---|
44 | /* * V0.40: */
|
---|
45 | /* - Introduced object 0x1010, 0x1011 */
|
---|
46 | /* */
|
---|
47 | /* * V0.38: */
|
---|
48 | /* - Introduced object 0x100a */
|
---|
49 | /* */
|
---|
50 | /* * V0.37: */
|
---|
51 | /* - ON ERROR GOSUB moved after new init section */
|
---|
52 | /* */
|
---|
53 | /* * V0.36: */
|
---|
54 | /* - Enahnced Initialization (NOWAIT OFF, etc.) */
|
---|
55 | /* */
|
---|
56 | /* * V0.35: */
|
---|
57 | /* - SDO 0x4003/EXIT introduced */
|
---|
58 | /* */
|
---|
59 | /* * V0.34: */
|
---|
60 | /* - PDO1 as answer to a SDO added (maybe SDO changes state) */
|
---|
61 | /* */
|
---|
62 | /* ----------------------------------------------------------------------- */
|
---|
63 |
|
---|
64 | PRINT "Magic Mics V", kVERSION, ".", kSUBVERSION /* */
|
---|
65 |
|
---|
66 | /* ----------------------------------------------------------------------- */
|
---|
67 | /* */
|
---|
68 | /* Object Dictionary: */
|
---|
69 | /* */
|
---|
70 | /* 0x1003 x rw Read delete error list (subidx 0-9) */
|
---|
71 | /* 0x1004 0 ro Nr of PDOs (transmit) */
|
---|
72 | /* 1 ro Nr of PDOs (synchron) */
|
---|
73 | /* 2 ro Nr of PDOs (asynchron) */
|
---|
74 | /* 0x1005 x ro COB ID for Syncs */
|
---|
75 | /* 0x100a x ro Software Version */
|
---|
76 | /* 0x100b x ro Node number */
|
---|
77 | /* 0x100e x ro COB ID for Guarding */
|
---|
78 | /* 0x1010 x wo Write data to EEPROM */
|
---|
79 | /* 0x1014 x ro COB ID for Emergency */
|
---|
80 | /* 0x1800 x rw Enable PDO1 (Axe Status, Position) */
|
---|
81 | /* 0x2000 0 rw Maximum positioning error */
|
---|
82 | /* 1 rw Negative Software Endswitch Value (Set=Enable) */
|
---|
83 | /* 2 rw Positive Software Endswitch Value (Set=Enable) */
|
---|
84 | /* 3 rw Enable/Disable Software Endswitch */
|
---|
85 | /* 0x2002 x rw Velocity */
|
---|
86 | /* 0x2003 0 wo Acceleration */
|
---|
87 | /* 1 wo Deceleration */
|
---|
88 | /* 0x3000 x wo Motor 'on', 'off', 'stop' */
|
---|
89 | /* 0x3001 x wo Home 'home' */
|
---|
90 | /* 0x3002 x wo Reopen Communication 'open' */
|
---|
91 | /* 0x3003 x wo Exit Program 'exit' */
|
---|
92 | /* 0x3006 0 wo Velocity Mode 'strt', 'stop' */
|
---|
93 | /* 1 wo VelMode Velocity */
|
---|
94 | /* 0x3007 x wo Syncronisation 'sync' */
|
---|
95 | /* 0x3008 x wo Nowait 'on', 'off' */
|
---|
96 | /* 0x6000 x rw Rotation Direction */
|
---|
97 | /* 0x6002 x rw Velocity Resolution */
|
---|
98 | /* 0x6003 0 wo Define present position as origin ('set') */
|
---|
99 | /* 1 wo Define new origin (0=delete) */
|
---|
100 | /* 2 rw Home Offset */
|
---|
101 | /* 0x6004 0 rw Absolute Position */
|
---|
102 | /* 1 wo Relative Position */
|
---|
103 | /* 1 ro Control Position */
|
---|
104 | /* 0x6501 x rw Encoder Resolution */
|
---|
105 | /* 0x6502 x rw Maximum Velocity */
|
---|
106 | /* 0x6508 x ro Time since switch on */
|
---|
107 | /* */
|
---|
108 | /* ----------------------------------------------------------------------- */
|
---|
109 |
|
---|
110 |
|
---|
111 | /*-------------------------------------------------------------------------*/
|
---|
112 | /* section for global constants */
|
---|
113 | /*-------------------------------------------------------------------------*/
|
---|
114 | SET PRGPAR 0 /* Restart Program on Exit */
|
---|
115 |
|
---|
116 | SET ENCODERTYPE 0 /* Incremental Encoder */
|
---|
117 | SET ENCODER 500 /* Encoder has 500 Ticks */
|
---|
118 | SET MENCODERTYPE 0 /* Incremental Encoder (Master) */
|
---|
119 | SET MENCODER 500 /* Encoder has 500 Ticks (Master) */
|
---|
120 | SET VELMAX 3600 /* Motor: Maximum rounds per minute */
|
---|
121 | SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */
|
---|
122 | SET ENDSWMOD 1 /* At End Switch Stop Motor with Max Decel. */
|
---|
123 | SET ERRCOND 2 /* Motor Stop */
|
---|
124 | SET POSDRCT 1 /* rotation direction */
|
---|
125 | SET POSFACT_Z 1 /* 1 user unit (be) = POSFACT_Z/POSFACT_N qc */
|
---|
126 | SET POSFACT_N 1 /* */
|
---|
127 | SET HOME_FORCE 1 /* Force Home positioning on startup */
|
---|
128 | SET HOME_OFFSET 0 /* Offset between index and home position */
|
---|
129 | SET HOMETYPE 0 /* drive to home, reverse, go to next index */
|
---|
130 | SET RAMPTYPE 0 /* Ramp Type: 0=Trapez, 1=Sinus */
|
---|
131 |
|
---|
132 | /*----------------*/
|
---|
133 | /* syncronisation */
|
---|
134 | /*----------------*/
|
---|
135 | SET MENCODERTYPE 0 /* Incremental Encoder (Master) */
|
---|
136 | SET MENCODER 500 /* Encoder has 500 Ticks (Master) */
|
---|
137 | SET SYNCFACTM 1 /* Master Sync Velocity factor */
|
---|
138 | SET SYNCFACTS 1 /* Slave Sync Velocity factor */
|
---|
139 | SET SYNCPOSOFFS 0 /* Sync Position offset between M/S */
|
---|
140 | SET SYNCACCURACY 50 /* When to set Accuracy Flag */
|
---|
141 | SET REVERS 0 /* How to handle reversation of vel */
|
---|
142 |
|
---|
143 | /*----------------*/
|
---|
144 | /* Inputs */
|
---|
145 | /*----------------*/
|
---|
146 | SET I_REFSWITCH -2 /* Reference Switch, Input 2, leading edge */
|
---|
147 | SET I_POSLIMITSW -2 /* Pos Limit Switch, Input 2, leading edge */
|
---|
148 | SET I_NEGLIMITSW -1 /* Neg Limit Switch, Input 1, leading edge */
|
---|
149 | SET I_BREAK 0 /* Input which brakes a running program */
|
---|
150 | SET I_CONTINUE 0 /* Input to continue a broken program */
|
---|
151 | SET I_ERRCLR 0 /* Input to clear error */
|
---|
152 |
|
---|
153 | /*----------------*/
|
---|
154 | /* Outputs */
|
---|
155 | /*----------------*/
|
---|
156 | SET O_AXMOVE 0 /* Motor control is working */
|
---|
157 | SET O_BRAKE 0 /* Brake */
|
---|
158 | SET O_ERROR 0 /* error occured */
|
---|
159 |
|
---|
160 | /*----------------*/
|
---|
161 | /* Dflt vel & acc */
|
---|
162 | /*----------------*/
|
---|
163 | vres = (GET ENCODER)*(GET VELMAX) /* ticks/R * R/M = ticks/min */
|
---|
164 | SET VELRES vres /* Set velocity units */
|
---|
165 | SET HOME_VEL -(25*vres%100) /* Home position velocity: 25% */
|
---|
166 | SET HOME_RAMP (25*vres%100) /* Home position accel: 25% */
|
---|
167 | SET DFLTACC (10*vres%100) /* Default acceleratio: 10% */
|
---|
168 | SET DFLTVEL (10*vres%100) /* Default velocity: 10% */
|
---|
169 |
|
---|
170 | /*----------------*/
|
---|
171 | /* Software range */
|
---|
172 | /*----------------*/
|
---|
173 | SET SWPOSLIMACT 0 /* positive software limit switch inactive */
|
---|
174 | SET SWNEGLIMACT 0 /* negative software limit switch inactive */
|
---|
175 | SET POSLIMIT 0 /* positive software limit (qc) */
|
---|
176 | SET NEGLIMIT 0 /* negative software limit (qc) */
|
---|
177 |
|
---|
178 | /*-------------------------------------------------------------------------*/
|
---|
179 | /* const section for constant velues */
|
---|
180 | /*-------------------------------------------------------------------------*/
|
---|
181 | kTRUE = 1
|
---|
182 | kFALSE = 0
|
---|
183 |
|
---|
184 | pdotime = 100
|
---|
185 | pdo1on = kFALSE
|
---|
186 |
|
---|
187 | /*-------------------------------------------------------------------------*/
|
---|
188 | /* Can Open Difinitions */
|
---|
189 | /*-------------------------------------------------------------------------*/
|
---|
190 | CANDEL -1
|
---|
191 | nodenr = GET CANNR
|
---|
192 | PRINT "Initializing Node Nr.", nodenr
|
---|
193 | pdo1 = DEFCANOUT (0x180+nodenr) 8
|
---|
194 | pdo2 = DEFCANOUT (0x280+nodenr) 8
|
---|
195 | sdotx = DEFCANOUT (0x580+nodenr) 8
|
---|
196 | sdorx = DEFCANIN (0x600+nodenr) 8
|
---|
197 |
|
---|
198 | err = REOPEN 0 0
|
---|
199 |
|
---|
200 | /*-------------------------------------------------------------------------*/
|
---|
201 | /* Init */
|
---|
202 | /*-------------------------------------------------------------------------*/
|
---|
203 | MOTOR STOP
|
---|
204 | MOTOR OFF
|
---|
205 | CVEL 0
|
---|
206 | NOWAIT OFF
|
---|
207 | OUT 1 1
|
---|
208 |
|
---|
209 | /*-------------------------------------------------------------------------*/
|
---|
210 | /* ON ... GOSUB ... definitions */
|
---|
211 | /*-------------------------------------------------------------------------*/
|
---|
212 | /* ON CANMSG GOSUB PROC_CANMSG */
|
---|
213 | i = 9
|
---|
214 | while (i) do
|
---|
215 | errlist[i] = 0
|
---|
216 | i = i - 1
|
---|
217 | endwhile
|
---|
218 |
|
---|
219 | ON ERROR GOSUB PROC_ERROR
|
---|
220 |
|
---|
221 | /*-------------------------------------------------------------------------*/
|
---|
222 | /* Program Main Loop */
|
---|
223 | /*-------------------------------------------------------------------------*/
|
---|
224 | MAIN:
|
---|
225 | canhi = 0
|
---|
226 | canlo = 1
|
---|
227 |
|
---|
228 | PRINT "Starting Mainloop..."
|
---|
229 |
|
---|
230 | MAINLOOP:
|
---|
231 | rc = CANIN sdorx 0 0 canhi canlo
|
---|
232 | if (rc==0) then /* It must be tested because ON PERIOD breaks 'wait for obj' */
|
---|
233 | gosub PROC_SDORX
|
---|
234 | endif
|
---|
235 | goto MAINLOOP
|
---|
236 | ENDMAIN:
|
---|
237 | MOTOR STOP
|
---|
238 | MOTOR OFF
|
---|
239 | OUT 1 0
|
---|
240 | EXIT
|
---|
241 |
|
---|
242 | /*-------------------------------------------------------------------------*/
|
---|
243 | /* Part for Programs called with GOSUB */
|
---|
244 | /*-------------------------------------------------------------------------*/
|
---|
245 |
|
---|
246 | SUBMAINPROG
|
---|
247 | /*----------------------------------*/
|
---|
248 | /* PROC_CANOPENMSG */
|
---|
249 | /*----------------------------------*/
|
---|
250 | SUBPROG PROC_SDOSET
|
---|
251 | idx = canhi&0xff00 | (canhi>>16)&0xff
|
---|
252 | subidx = canhi&0xff
|
---|
253 | sdoval = (canlo&0xff)<<24 | (canlo&0xff00)<<8 | (canlo>>8)&0xff00 | (canlo>>24)&0xff
|
---|
254 | /*
|
---|
255 | PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval
|
---|
256 | */
|
---|
257 | if (idx == 0x1003 and subidx == 0 and sdoval == 0) then
|
---|
258 | i = 9
|
---|
259 | while (i) do
|
---|
260 | errlist[i] = 0
|
---|
261 | i = i - 1
|
---|
262 | endwhile
|
---|
263 | elseif (idx == 0x1010 and sdoval == 's'<<24|'a'<<16|'v'<<8|'e') then
|
---|
264 | SAVEPROM
|
---|
265 | elseif (idx == 0x1800 and subidx == 1) then
|
---|
266 | ON PERIOD 0 GOSUB PROC_PDO1
|
---|
267 | if (sdoval>>31) then
|
---|
268 | pdo1on = kFALSE
|
---|
269 | else
|
---|
270 | ON PERIOD pdotime GOSUB PROC_PDO1
|
---|
271 | pdo1on = kTRUE
|
---|
272 | endif
|
---|
273 | elseif (idx == 0x2000) then
|
---|
274 | if (subidx == 0) then
|
---|
275 | SET POSERR sdoval
|
---|
276 | elseif (subidx == 1) then
|
---|
277 | SET NEGLIMIT sdoval
|
---|
278 | SET SWNEGLIMACT 1
|
---|
279 | elseif (subidx == 2) then
|
---|
280 | SET POSLIMIT sdoval
|
---|
281 | SET SWPOSLIMACT 1
|
---|
282 | elseif (subidx == 3) then
|
---|
283 | SET SWNEGLIMACT sdoval&1
|
---|
284 | SET SWPOSLIMACT (sdoval>>1)&1
|
---|
285 | endif
|
---|
286 | elseif (idx == 0x2002) then
|
---|
287 | VEL sdoval
|
---|
288 | elseif (idx == 0x2003) then
|
---|
289 | if (subidx) then
|
---|
290 | DEC sdoval
|
---|
291 | else
|
---|
292 | ACC sdoval
|
---|
293 | endif
|
---|
294 | elseif (idx == 0x3000) then
|
---|
295 | if (sdoval == 'o'<<24|'n'<<16) then
|
---|
296 | MOTOR ON
|
---|
297 | elseif (sdoval == 'o'<<24|'f'<<16|'f'<<8) then
|
---|
298 | MOTOR OFF
|
---|
299 | elseif (sdoval == 's'<<24|'t'<<16|'o'<<8|'p') then
|
---|
300 | MOTOR STOP
|
---|
301 | endif
|
---|
302 | elseif (idx == 0x3001) then
|
---|
303 | if (sdoval == 'h'<<24|'o'<<16|'m'<<8|'e') then
|
---|
304 | limitsw = GET I_POSLIMITSW
|
---|
305 | set I_POSLIMITSW 0
|
---|
306 | /* Disable Interrupt - same problem than with CANIN */
|
---|
307 | ON PERIOD 0 GOSUB PROC_PDO1
|
---|
308 | HOME
|
---|
309 | /* Reenable interrupt */
|
---|
310 | if (pdo1on) then
|
---|
311 | ON PERIOD pdotime GOSUB PROC_PDO1
|
---|
312 | endif
|
---|
313 | SET I_POSLIMITSW limitsw
|
---|
314 | endif
|
---|
315 | elseif (idx == 0x3002 and sdoval == 'o'<<24|'p'<<16|'e'<<8|'n') then
|
---|
316 | sdoval=REOPEN 2 0
|
---|
317 | if (sdoval) then
|
---|
318 | PRINT "Error Reopen"
|
---|
319 | endif
|
---|
320 | elseif (idx == 0x3003 and sdoval == 'e'<<24|'x'<<16|'i'<<8|'t') then
|
---|
321 | CANOUT sdotx (canhi&0xffffff | 0x60000000) 0
|
---|
322 | EXIT
|
---|
323 | elseif (idx == 0x3006) then
|
---|
324 | if (subidx == 0) then
|
---|
325 | if (sdoval == 's'<<24|'t'<<16|'r'<<8|'t') then
|
---|
326 | CVEL 0
|
---|
327 | CSTART
|
---|
328 | elseif (sdoval == 's'<<24|'t'<<16|'o'<<8|'p') then
|
---|
329 | CSTOP
|
---|
330 | endif
|
---|
331 | elseif (subidx == 1) then
|
---|
332 | CVEL sdoval
|
---|
333 | endif
|
---|
334 | elseif (idx == 0x3007 and sdoval == 's'<<24|'y'<<16|'n'<<8|'c') then
|
---|
335 | SYNCP
|
---|
336 | elseif (idx == 0x3008) then
|
---|
337 | if (sdoval == 'o'<<24|'n'<<16) then
|
---|
338 | NOWAIT ON
|
---|
339 | elseif (sdoval == 'o'<<24|'f'<<16|'f'<<8) then
|
---|
340 | NOWAIT OFF
|
---|
341 | endif
|
---|
342 | /* elseif (idx == 0x4000 and
|
---|
343 | (sdoval>>24)&0xff == 'S' and
|
---|
344 | (sdoval>>16)&0xff == 'T' and
|
---|
345 | (sdoval>>8) &0xff == 'O' and
|
---|
346 | (sdoval) &0xff == 'P') then
|
---|
347 | CANOUT sdotx (canhi&0xffffff | 0x60000000) 0
|
---|
348 | goto ENDMAIN
|
---|
349 | */ elseif (idx == 0x6000) then
|
---|
350 | if (sdoval&1) then
|
---|
351 | SET POSDRCT -1
|
---|
352 | else
|
---|
353 | SET POSDRCT 1
|
---|
354 | endif
|
---|
355 | elseif (idx == 0x6002) then
|
---|
356 | SET VELRES sdoval
|
---|
357 | elseif (idx == 0x6003) then
|
---|
358 | if (subidx == 0 and sdoval == 's'<<24|'e'<<16|'t'<<8) then
|
---|
359 | DEF ORIGIN
|
---|
360 | elseif (subidx == 1) then
|
---|
361 | if (sdoval==0) then
|
---|
362 | RST ORIGIN
|
---|
363 | else
|
---|
364 | SET ORIGIN sdoval
|
---|
365 | endif
|
---|
366 | elseif (subidx == 2) then
|
---|
367 | SET HOME_OFFSET sdoval
|
---|
368 | endif
|
---|
369 | elseif (idx == 0x6004) then
|
---|
370 | if (subidx==0) then
|
---|
371 | POSA sdoval
|
---|
372 | elseif (subidx==1) then
|
---|
373 | POSR sdoval
|
---|
374 | endif
|
---|
375 | elseif (idx == 0x6200) then
|
---|
376 | pdotime = sdoval
|
---|
377 | if (pdo1on) then
|
---|
378 | ON PERIOD 0 GOSUB PROC_PDO1
|
---|
379 | ON PERIOD pdotime GOSUB PROC_PDO1
|
---|
380 | endif
|
---|
381 | elseif (idx == 0x6501) then
|
---|
382 | SET ENCODER sdoval
|
---|
383 | elseif (idx == 0x6502) then
|
---|
384 | SET VELMAX sdoval
|
---|
385 | else
|
---|
386 | CANOUT sdotx (canhi&0xffffff | 0x80000000) 0
|
---|
387 | goto ENDSDOSET
|
---|
388 | endif
|
---|
389 |
|
---|
390 | if (pdo1on) then
|
---|
391 | GOSUB PROC_PDO1
|
---|
392 | endif
|
---|
393 |
|
---|
394 | CANOUT sdotx (canhi&0xffffff | 0x60000000) 0
|
---|
395 | /*
|
---|
396 | PRINT "Sdo Set ", idx, "/", subidx
|
---|
397 | */
|
---|
398 | ENDSDOSET:
|
---|
399 | RETURN
|
---|
400 |
|
---|
401 | /*----------------------------------*/
|
---|
402 |
|
---|
403 | SUBPROG PROC_SDOREQ
|
---|
404 | idx = canhi&0xff00 | (canhi>>16)&0xff
|
---|
405 | subidx = canhi&0xff
|
---|
406 | /*
|
---|
407 | PRINT "Requesting Idx:", idx, "/", subidx
|
---|
408 | */
|
---|
409 | if (idx == 0x1003) then
|
---|
410 | if (subidx >=0 and subidx<=9) then
|
---|
411 | sdoval = errlist[subidx-1]
|
---|
412 | endif
|
---|
413 | elseif (idx == 0x1004) then
|
---|
414 | if (subidx == 0) then
|
---|
415 | sdoval = 1
|
---|
416 | elseif (subidx == 1) then
|
---|
417 | sdoval = 0
|
---|
418 | elseif (subidx == 2) then
|
---|
419 | sdoval = 1
|
---|
420 | endif
|
---|
421 | elseif (idx == 0x1005) then
|
---|
422 | sdoval = 1<<31 | 0x80
|
---|
423 | elseif (idx == 0x100b) then
|
---|
424 | sdoval = nodenr
|
---|
425 | elseif (idx == 0x100a) then
|
---|
426 | sdoval = (kVERSION<<16) | kSUBVERSION
|
---|
427 | elseif (idx == 0x100e) then
|
---|
428 | sdoval = 0x700 | nodenr
|
---|
429 | elseif (idx == 0x1010) then
|
---|
430 | sdoval = 1
|
---|
431 | elseif (idx == 0x1011) then
|
---|
432 | sdoval = 0
|
---|
433 | elseif (idx == 0x1014) then
|
---|
434 | sdoval = 0x80 | nodenr
|
---|
435 | elseif (idx == 0x1800) then
|
---|
436 | if (subidx == 1) then
|
---|
437 | sdoval = (~pdo1on)<<31 | (0x0180 + nodenr)
|
---|
438 | elseif (subidx == 2) then
|
---|
439 | sdoval = 0xfe
|
---|
440 | elseif (subidx == 3) then
|
---|
441 | sdoval = 0
|
---|
442 | endif
|
---|
443 | elseif (idx == 0x2000) then
|
---|
444 | if (subidx == 0) then
|
---|
445 | sdoval = GET POSERR
|
---|
446 | elseif (subidx == 1) then
|
---|
447 | sdoval = GET NEGLIMIT | (GET SWNEGLIMACT) << 30
|
---|
448 | elseif (subidx == 2) then
|
---|
449 | sdoval = GET POSLIMIT | (GET SWPOSLIMACT) << 31
|
---|
450 | elseif (subidx == 3) then
|
---|
451 | sdoval = (GET SWNEGLIMACT) | ((GET SWPOSLIMACT)<<1)
|
---|
452 | endif
|
---|
453 | elseif (idx == 0x2001) then
|
---|
454 | sdoval = AXEND
|
---|
455 | elseif (idx == 0x2002) then
|
---|
456 | sdoval = AVEL
|
---|
457 | elseif (idx == 0x2003) then
|
---|
458 | if (subidx==0) then
|
---|
459 | sdoval = INB 0
|
---|
460 | elseif (subidx>0 and subidx<2) then
|
---|
461 | sdoval = IN subidx
|
---|
462 | endif
|
---|
463 | elseif (idx == 0x2004) then
|
---|
464 | sdoval = STAT
|
---|
465 | elseif (idx == 0x4000) then
|
---|
466 | WAITAX
|
---|
467 | elseif (idx == 0x6000) then
|
---|
468 | if (GET POSDRCT == 1) then
|
---|
469 | sdoval = 0
|
---|
470 | elseif (GET POSDRCT == -1) then
|
---|
471 | sdoval = 1
|
---|
472 | endif
|
---|
473 | elseif (idx == 0x6002) then
|
---|
474 | sdoval = GET VELRES
|
---|
475 | elseif (idx == 0x6003) then
|
---|
476 | sdoval = GET HOME_OFFSET
|
---|
477 | elseif (idx == 0x6004) then
|
---|
478 | if (subidx == 0) then
|
---|
479 | sdoval = APOS
|
---|
480 | elseif (subidx==1) then
|
---|
481 | sdoval = CPOS
|
---|
482 | endif
|
---|
483 | elseif (idx == 0x6501) then
|
---|
484 | sdoval = GET ENCODER
|
---|
485 | elseif (idx == 0x6502) then
|
---|
486 | sdoval = GET VELMAX
|
---|
487 | elseif (idx == 0x6508) thenä
|
---|
488 | sdoval = TIME
|
---|
489 | else
|
---|
490 | CANOUT sdotx (canhi&0xffffff | 0x80000000) 0
|
---|
491 | goto ENDSDOREQ
|
---|
492 | endif
|
---|
493 |
|
---|
494 | canlo = (sdoval&0xff)<<24 | (sdoval&0xff00)<<8 | (sdoval>>8)&0xff00 | (sdoval>>24)&0xff
|
---|
495 | CANOUT sdotx (canhi&0xffffff | 0x43000000) canlo
|
---|
496 | /*
|
---|
497 | PRINT "Returning: ", sdoval
|
---|
498 | */
|
---|
499 | ENDSDOREQ:
|
---|
500 | RETURN
|
---|
501 |
|
---|
502 | /*----------------------------------*/
|
---|
503 |
|
---|
504 | SUBPROG PROC_SDORX
|
---|
505 | /* --Echo--
|
---|
506 | CANOUT sdotx canhi canlo
|
---|
507 | */
|
---|
508 | cmd = canhi>>24
|
---|
509 | if (cmd==0x23 OR cmd==0x2B OR cmd==0x2F) then
|
---|
510 | gosub PROC_SDOSET
|
---|
511 | elseif (cmd == 0x40) then
|
---|
512 | gosub PROC_SDOREQ
|
---|
513 | else
|
---|
514 | PRINT "Unknown SDO cmd", cmd
|
---|
515 | CANOUT sdotx (canhi&0xffffff | 0x80000000) 0
|
---|
516 | endif
|
---|
517 | ENDSDORX:
|
---|
518 | RETURN
|
---|
519 |
|
---|
520 | /*----------------------------------*/
|
---|
521 | /* PROC_CANMSG */
|
---|
522 | /* called if a canmsg with */
|
---|
523 | /* cobid=2*CANNR+1 is received */
|
---|
524 | /* Warning: This doesn't fit to */
|
---|
525 | /* CanOpen specification */
|
---|
526 | /*----------------------------------*/
|
---|
527 | SUBPROG PROC_CANMSG
|
---|
528 | varnr = InMsg(-1)
|
---|
529 | PRINT "varnr=", varnr, "msgval=", msgval
|
---|
530 | RETURN
|
---|
531 |
|
---|
532 | /*-------------------------------------------------------------------------*/
|
---|
533 | /* PDO 1 Interrupt */
|
---|
534 | /*-------------------------------------------------------------------------*/
|
---|
535 | SUBPROG PROC_PDO1
|
---|
536 | CANOUT pdo1 AXEND APOS
|
---|
537 | RETURN
|
---|
538 |
|
---|
539 | /*-------------------------------------------------------------------------*/
|
---|
540 | /* Error sub proc */
|
---|
541 | /*-------------------------------------------------------------------------*/
|
---|
542 | SUBPROG PROC_ERROR
|
---|
543 | MOTOR STOP
|
---|
544 |
|
---|
545 | /* Tell the bus that an error occured */
|
---|
546 | CANOUT pdo2 0 0
|
---|
547 |
|
---|
548 | i = errlist[1] + 1 /* Fill status of array */
|
---|
549 | while (i>2) do /* shift errors by one */
|
---|
550 | errlist[i] = errlist[i-1]
|
---|
551 | i = i - 1
|
---|
552 | endwhile /* set new errornumber */
|
---|
553 | errlist[2] = ERRNO
|
---|
554 | if (errlist[1]<8) then /* write new size if enhanced */
|
---|
555 | errlist[1] = errlist[1] + 1
|
---|
556 | endif
|
---|
557 |
|
---|
558 | errinf = 0
|
---|
559 |
|
---|
560 | /* check if the error is repairable and repair */
|
---|
561 | if (errlist[2]==6) then
|
---|
562 | PRINT "No home forced!"
|
---|
563 | ERRCLR
|
---|
564 | elseif (errlist[2]==8) then
|
---|
565 | PRINT "Control deviation overflow."
|
---|
566 | ERRCLR
|
---|
567 | errinf = 0xaffe
|
---|
568 | elseif (errlist[2]==9) then
|
---|
569 | PRINT "Did'n find zero index."
|
---|
570 | ERRCLR
|
---|
571 | elseif (errlist[2]==11) then
|
---|
572 | poslsw = GET POSLIMIT
|
---|
573 | neglsw = GET NEGLIMIT
|
---|
574 | if ((GET SWNEGLIMACT) and APOS<=neglsw) then
|
---|
575 | PRINT "Negative software endswitch (", neglsw, ") activated at position ", APOS
|
---|
576 | SET SWNEGLIMACT 0
|
---|
577 | ERRCLR
|
---|
578 | CVEL (vres%100)
|
---|
579 | ACC (10*vres%100)
|
---|
580 | DEC (10*vres%100)
|
---|
581 | POSA neglsw + 100
|
---|
582 | SET SWNEGLIMACT 1
|
---|
583 | errinf = -1
|
---|
584 | elseif ((GET SWPOSLIMACT) and APOS>=poslsw) then
|
---|
585 | PRINT "Positive software endswitch (", poslsw, ") activated at position ", APOS
|
---|
586 | SET SWPOSLIMACT 0
|
---|
587 | ERRCLR
|
---|
588 | CVEL (vres%100) /* 1% */
|
---|
589 | ACC (10*vres%100) /* 10% */
|
---|
590 | DEC (10*vres%100) /* 10% */
|
---|
591 | POSA poslsw - 100
|
---|
592 | SET SWPOSLIMACT 1
|
---|
593 | errinf = 1
|
---|
594 | else
|
---|
595 | PRINT "Software endswitch activated - command skipped. Pos: ", APOS
|
---|
596 | ERRCLR
|
---|
597 | endif
|
---|
598 | elseif (errlist[2]==25) then
|
---|
599 | /* FIXME: To handle this correct you must make sure,
|
---|
600 | that the endswitch numbers are negative */
|
---|
601 | poslsw = -(GET I_POSLIMITSW)
|
---|
602 | neglsw = -(GET I_NEGLIMITSW)
|
---|
603 | if (IN poslsw == 0) then
|
---|
604 | PRINT "Positive endswitch activated at position ", APOS
|
---|
605 | SET I_POSLIMITSW 0
|
---|
606 | ERRCLR
|
---|
607 | CVEL (vres%100) /* 1% */
|
---|
608 | ACC (10*vres%100) /* 10% */
|
---|
609 | DEC (10*vres%100) /* 10% */
|
---|
610 | CSTART
|
---|
611 | WHILE (IN poslsw == 0) DO ENDWHILE
|
---|
612 | CSTOP
|
---|
613 | SET I_POSLIMITSW -poslsw
|
---|
614 | errinf = 1
|
---|
615 | elseif (IN neglsw == 0) then
|
---|
616 | PRINT "Negative endswitch activated at position ", APOS
|
---|
617 | SET I_NEGLIMITSW 0
|
---|
618 | ERRCLR
|
---|
619 | vres = GET VELRES
|
---|
620 | CVEL -(vres%100) /* 1% */
|
---|
621 | ACC (10*vres%100) /* 10% */
|
---|
622 | DEC (10*vres%100) /* 10% */
|
---|
623 | CSTART
|
---|
624 | WHILE (IN poslsw == 0) DO ENDWHILE
|
---|
625 | CSTOP
|
---|
626 | SET I_NEGLIMITSW -poslsw
|
---|
627 | errinf = -1
|
---|
628 | endif
|
---|
629 | elseif (errlist[2]==84) then
|
---|
630 | PRINT "Too many (>12) ON TIME interrupts."
|
---|
631 | ERRCLR
|
---|
632 | ELSE
|
---|
633 | PRINT "Error Function Called: ERRNO=", errlist[2]
|
---|
634 | endif
|
---|
635 |
|
---|
636 | /* tell the bus what exactly happened */
|
---|
637 | CANOUT pdo2 errlist[2] errinf
|
---|
638 | RETURN
|
---|
639 |
|
---|
640 | /*-------------------------------------------------------------------------*/
|
---|
641 | /* End of part for Programs called with GOSUB */
|
---|
642 | /*-------------------------------------------------------------------------*/
|
---|
643 |
|
---|
644 | ENDPROG
|
---|