source: trunk/MagicSoft/Cosy/aposs/Magic.m@ 750

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