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

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