source: trunk/Cosy/aposs/Model.m@ 13316

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