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

Last change on this file since 8872 was 8872, checked in by tbretz, 17 years ago
*** empty log message ***
File size: 40.6 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 = 70 /* */
13/* */
14/* HISTORY: */
15/* */
16/* * V0.70: */
17/* - Implemented switch off of motor in case of Input 6 has a */
18/* falling edge */
19/* - moved the arbitrary factor 4 for the tracking speed from */
20/* cosy to here */
21/* */
22/* * V0.69: */
23/* - Implemented Pre-Operational/Operational state */
24/* means: + cannot start RPM mode */
25/* + SDO 0x6004 (POSA/POSR) ignored */
26/* + new SDO 0x1000 */
27/* */
28/* * V0.68: */
29/* - Made NoWait=On the default */
30/* - Implemented error 89 (CAN I/O) */
31/* - changed "REOPEN 0 0" to "REOPEN 1 0" */
32/* */
33/* * V0.67: */
34/* - trying to implement the control cabinet stuff */
35/* */
36/* * V0.66: */
37/* - implemented canopen node guarding (0x4000) */
38/* - implemented some kind of host guarding (in conjuction with */
39/* the implemented nodeguarding) */
40/* - removed old guarding using SDO 0x4000 */
41/* - implemented SDO 0x100c (guardtime) */
42/* - implemented SDO 0x100d (lifetimefactor) */
43/* */
44/* * V0.65: */
45/* - fixed a bug in the handling of the endswitches */
46/* */
47/* * V0.64: */
48/* - removed FORCEHOME */
49/* */
50/* * V0.63: */
51/* - added movement handshake timeout (removed 0x400 WAITAX) */
52/* - added brackets around string SDOs */
53/* - changed SDO 0x3007 to support both sync modes */
54/* */
55/* * V0.62: */
56/* - changed handling of 0x2000/1/2 added /3 */
57/* */
58/* * V0.61: */
59/* - corrected problems with the error handling */
60/* */
61/* * V0.60: */
62/* - introduced syncronisation */
63/* */
64/* * V0.52: */
65/* - changed the handling of the endswitch error (unknown switch) */
66/* */
67/* * V0.51: */
68/* - made errlist working */
69/* */
70/* * V0.50: */
71/* - Rearanged many object numbers */
72/* - Added object dictonary to comments */
73/* */
74/* * V0.43: */
75/* - Added Object 0x1003, 0x1004, 0x1005 */
76/* */
77/* * V0.42: */
78/* - Added APOS to PDO1 */
79/* */
80/* * V0.41: */
81/* - Period Interrupt diabled while HOME */
82/* */
83/* * V0.40: */
84/* - Introduced object 0x1010, 0x1011 */
85/* */
86/* * V0.38: */
87/* - Introduced object 0x100a */
88/* */
89/* * V0.37: */
90/* - ON ERROR GOSUB moved after new init section */
91/* */
92/* * V0.36: */
93/* - Enahnced Initialization (NOWAIT OFF, etc.) */
94/* */
95/* * V0.35: */
96/* - SDO 0x4003/EXIT introduced */
97/* */
98/* * V0.34: */
99/* - PDO1 as answer to a SDO added (maybe SDO changes state) */
100/* */
101/* ----------------------------------------------------------------------- */
102
103PRINT "Magic Mics V", kVERSION, ".", kSUBVERSION, " (CANbus controlled)" /**/
104
105/* ----------------------------------------------------------------------- */
106/* */
107/* Object Dictionary: */
108/* */
109/* 0x1000 1 rw Start node (Pre-oprational --> operational) */
110/* 0x1003 x rw Read delete error list (subidx 0-9) */
111/* 0x1004 0 ro Nr of PDOs (transmit) */
112/* 1 ro Nr of PDOs (synchron) */
113/* 2 ro Nr of PDOs (asynchron) */
114/* 0x1005 x ro COB ID for Syncs */
115/* 0x100a x ro Software Version */
116/* 0x100b x ro Node number */
117/* 0x100c x rw Guardtime (ms) */
118/* 0x100d x rw Lifetime factor */
119/* 0x100e x ro COB ID for Guarding */
120/* 0x1010 x wo Write data to EEPROM */
121/* 0x1014 x ro COB ID for Emergency */
122/* 0x1800 x rw Enable PDO1 (Axe Status, Position) */
123/* 0x2000 0 rw Maximum positioning error */
124/* 1 rw Negative Software Endswitch Value (Set=Enable) */
125/* 2 rw Positive Software Endswitch Value (Set=Enable) */
126/* 3 rw Enable/Disable Software Endswitch */
127/* 0x2002 x rw Velocity */
128/* 0x2003 0 wo Acceleration */
129/* 1 wo Deceleration */
130/* 0x3000 x wo Motor 'on', 'off', 'stop' (Warning: Motor off can have */
131/* strange side effects if RF=1!) */
132/* 0x3001 x wo Home 'home' */
133/* 0x3002 x wo Reopen Communication 'open' */
134/* 0x3003 x wo Exit Program 'exit' */
135/* 0x3006 0 wo Velocity Mode 'strt', 'stop' */
136/* 1 wo VelMode Velocity */
137/* 0x3007 0 wo Velocity Syncronisation 'sync' */
138/* 1 wo Position Syncronisation 'sync' */
139/* 0x3008 x wo Nowait 'on', 'off' */
140/* 0x4000 0 wo Reset timeout timer (Nodeguard) */
141/* 0x6000 x rw Rotation Direction */
142/* 0x6002 x rw Velocity Resolution */
143/* 0x6003 0 wo Define present position as origin ('set') */
144/* 1 wo Define new origin (0=delete) */
145/* 2 rw Home Offset */
146/* 0x6004 0 rw Absolute Position */
147/* 1 wo Relative Position */
148/* 1 ro Control Position */
149/* 0x6501 x rw Encoder Resolution */
150/* 0x6502 x rw Maximum Velocity */
151/* 0x6508 x ro Time since switch on */
152/* */
153/* ----------------------------------------------------------------------- */
154
155
156/*-------------------------------------------------------------------------*/
157/* section for global constants */
158/*-------------------------------------------------------------------------*/
159SET PRGPAR -1 /* Don't restart any Program on Exit */
160
161SET ENCODERTYPE 0 /* Incremental Encoder */
162SET MENCODERTYPE 0 /* Incremental Encoder (Master) */
163
164SET ENDSWMOD 0 /* No End Switch */
165SET ERRCOND 2 /* Motor Stop, position control, no break */
166SET POSDRCT -1 /* rotation direction */
167/* OLD: SET POSDRCT -1 /* rotation direction */
168SET POSFACT_Z 1 /* 1 user unit (be) = POSFACT_Z/POSFACT_N qc */
169SET POSFACT_N 1 /* */
170
171SET HOME_FORCE 0 /* Don't force Home positioning on mainloopup */
172SET HOME_OFFSET 0 /* Offset between index and home position */
173SET HOMETYPE 0 /* drive to home, reverse, go to next index */
174
175/*----------------*/
176/* syncronisation */
177/*----------------*/
178SET SYNCFACTM 1 /* Master Sync Velocity factor */
179SET SYNCFACTS 1 /* Slave Sync Velocity factor */
180SET SYNCPOSOFFS 0 /* Sync Position offset between M/S */
181SET SYNCACCURACY 50 /* When to set Accuracy Flag */
182SET REVERS 0 /* How to handle reversation of vel */
183
184/*----------------*/
185/* Inputs */
186/*----------------*/
187SET I_REFSWITCH 0 /* Reference Switch */
188SET I_POSLIMITSW 0 /* Pos Limit Switch */
189SET I_NEGLIMITSW 0 /* Neg Limit Switch */
190SET I_BREAK 0 /* Input which brakes a running program */
191SET I_CONTINUE 0 /* Input to continue a broken program */
192SET I_ERRCLR 0 /* Input to clear error */
193
194/*----------------*/
195/* Outputs */
196/*----------------*/
197SET O_AXMOVE 0 /* Motor control is working */
198SET O_BRAKE 0 /* Brake */
199SET O_ERROR 0 /* error occured */
200
201/*----------------*/
202/* Unit param. */
203/*----------------*/
204SET RAMPTYPE 1 /* Ramp Type: 0=Trapez, 1=Sinus */
205/* OLD: SET RAMPTYPE 0 /* Ramp Type: 0=Trapez, 1=Sinus */
206SET ENCODER 1500 /* Encoder has 1500 Ticks */
207SET MENCODER 1500 /* Encoder has 500 Ticks (Master) */
208SET VELMAX 3000 /* Motor: Maximum revolutions per minute */
209SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */
210SET RAMPMIN 10000 /* Shortest Ramp 10s */
211
212/*----------------*/
213/* Dflt vel & acc */
214/*----------------*/
215
216/* OLD ORIGINAL VALUES: Prop=100, Div=300, Int=1000 */
217if (get cannr==1) then
218SET KPROP 950
219SET KDER 2000
220SET KINT 0
221elseif (get cannr==2) then
222SET KPROP 100
223SET KDER 200
224SET KINT 150
225else
226SET KPROP 350
227SET KDER 50
228SET KINT 350
229endif
230
231vres = (GET ENCODER)*(GET VELMAX) /* ticks/R * R/M = ticks/min */
232SET VELRES vres /* Set velocity units */
233
234
235/* OLD: SET HOME_VEL -(25*vres%100) /* Home position velocity: 25% */
236/* OLD: SET HOME_RAMP (25*vres%100) /* Home position accel: 25% */
237
238/*----------------*/
239/* Manual control */
240/*----------------*/
241SET RAMPTYPE 1 /* Ramp: 0=linear, 1=sinus */
242defacc = 60*vres%100
243
244SET DFLTACC defacc /* Default acceleratio: [%] */
245ACC defacc
246DEC defacc*2
247 /* Velocity which is reached in
248 a time given by RAMPMIN */
249SET DFLTVEL (1*vres%100) /* Default velocity [%] */
250
251/*manvel = (4*vres%100) /* 150 U/min */ /* Max speed in man mode: [%] */
252
253print "Vel Res (vel max): ", GET VELRES, " Encoder Ticks/min"
254/*print "V_man: ", manvel, " Encoder Ticks/min"*/
255
256/*-------------------------------------------------------------------------*/
257/* const section for constant velues */
258/*-------------------------------------------------------------------------*/
259kTRUE = 1
260kFALSE = 0
261
262pdotime = 100
263pdo1on = kFALSE
264
265guardtime = 0
266lifetimefactor = 0
267timeouttime = TIME
268firsttimeout = 0
269
270preop = kTRUE
271
272/*-------------------------------------------------------------------------*/
273/* Can Open Definitions */
274/*-------------------------------------------------------------------------*/
275/* The CAN Object are static object. This is why they must be deleted. */
276/* The program should run in any of our nodes. */
277/* Therefor the standard CAN objects (SDo, PDO1, PDO2) for communication */
278/* are defined. The nodenumber is part of the object ID (this is somehow */
279/* similar to the TCP/IP ports) */
280/*-------------------------------------------------------------------------*/
281CANDEL -1
282
283nodenr = GET CANNR
284PRINT "Initializing Node Nr.", nodenr
285pdo1 = DEFCANOUT (0x180+nodenr) 8
286pdo2 = DEFCANOUT (0x280+nodenr) 8
287pdo3 = DEFCANOUT (0x380+nodenr) 8
288sdotx = DEFCANOUT (0x580+nodenr) 8
289sdorx = DEFCANIN (0x600+nodenr) 8
290/*
291 guardrx = DEFCANIN (0x700+nodenr) 8
292 guardtx = DEFCANOUT (0x700+nodenr) 8
293 */
294
295/* Close and reopen communication, enable buffering */
296err = REOPEN 1 0
297
298/*-------------------------------------------------------------------------*/
299/* Before the motor control hardware is enabled (hi on output 1) */
300/* the commands make sure, that the motor will not start moving. */
301/* As default positioning commands doesn't stop the further execution */
302/* of the program. */
303/*-------------------------------------------------------------------------*/
304/*MOTOR STOP
305MOTOR OFF
306CVEL 0
307OUT 1 1
308*/
309NOWAIT ON
310
311/*-------------------------------------------------------------------------*/
312/* ON ... GOSUB ... definitions */
313/*-------------------------------------------------------------------------*/
314/* The errorlist one can retreive using the corresponding CAN object */
315/* should be emty when the node is initialized (arrays are static objects) */
316/* therefor it must be deleted. */
317/* Errors are handled in an interrupt procedure called PROC_ERROR */
318/*-------------------------------------------------------------------------*/
319/* ON CANMSG GOSUB PROC_CANMSG */
320i = 9
321while (i) do
322 errlist[i] = 0
323 i = i - 1
324endwhile
325
326ON ERROR GOSUB PROC_ERROR
327
328/***************************************************************************/
329kIoModule = 4*256
330
331ON INT -(kIoModule+6) GOSUB DOOR_STATUS_CHANGED
332ON INT (kIoModule+6) GOSUB DOOR_STATUS_CHANGED
333
334/*-------------------------------------------------------------------------*/
335/* Init */
336/*-------------------------------------------------------------------------*/
337/* Program Main Loop */
338/*-------------------------------------------------------------------------*/
339/* The main loop is the core of the program which handles incoming */
340/* objects. In principal CANIN should wait until an object is received, */
341/* but it stops waiting when an interrupt occurs. This is the reason why */
342/* the validity of the message must be checked. */
343/*-------------------------------------------------------------------------*/
344MAIN:
345 canhi = 0
346 canlo = 1
347
348 brake = 0
349 RF = 0
350
351 init = 0
352
353 gosub reset
354
355 PRINT "Starting Mainloop..."
356
357 rc = 1
358 MAINLOOP:
359 if (rc==0) then /* It must be tested because ON PERIOD breaks 'wait for obj' */
360 gosub PROC_SDORX
361 else
362 fuse = in (kIoModule+1)
363 emcy = in (kIoModule+2)
364 vltg = in (kIoModule+3)
365 mode = in (kIoModule+4)
366 door = in (kIoModule+6)
367 ready = in 1
368
369 if (door==0) then
370 preop = kTRUE
371 endif
372
373 if (preop==kTRUE) then
374 print "Door open!"
375 gosub reset
376 elseif (ready==0) and (RF==1) then
377 print "DKC not ready, but RF set... setting RF=AH=0!"
378 gosub reset
379 goto mainloop
380 elseif (mode==1) then
381 print "Control not in PC mode!"
382 gosub reset
383 SET PRGPAR 0
384 exit
385 elseif (fuse==0) then
386 print "Motor-Power Fuse not OK!"
387 gosub reset
388 goto mainloop
389 elseif (vltg==0) then
390 print "Overvoltage control broken!"
391 gosub reset
392 goto mainloop
393 elseif (emcy==0) then
394 print "Please release Emergency Stop!"
395 gosub reset
396 goto mainloop
397 elseif (ready==1) and (RF==0) then
398 print "DKC powered, RF=0... setting RF=AH=1!"
399 /*
400 * After switching on power wait at least 300ms until
401 * control changed state 'bb' to 'ab'
402 */
403 cvel 0
404 waitt 300
405 out 1 0
406 out 2 0
407 motor off
408 waitt 100
409 out 1 1
410 out 2 1
411 RF = 1
412 waitt 100
413
414 if (brake==0 and get cannr==3) then
415 out (kIoModule+1) 1
416 brake = 1
417 waitt 1000
418 endif
419
420 /* This "motor on" is necessary to make sure the floating analog output
421 doesn't move the telescope (slowly) */
422 motor on
423
424 canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4) | (rf<<5) | (brake<<6)) 0
425 /*
426 if (get cannr==2) then
427 syncv
428 print "Synchronizing speed..."
429 else
430 print "Starting CAN mode..." ^
431 goto CANSTART
432 endif */
433 elseif (ready==0) or (RF==0) then
434 print "No Power, no RF..."
435 canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4) | (rf<5) | (brake<<6)) 0
436 waitt 500
437 goto mainloop
438 endif
439 endif
440
441 rc = CANIN sdorx -1 0 canhi canlo
442 goto mainloop
443
444ENDMAIN:
445 MOTOR STOP
446 MOTOR OFF
447 OUT 1 0
448 EXIT
449
450/*-------------------------------------------------------------------------*/
451/* Part for Programs called with GOSUB */
452/*-------------------------------------------------------------------------*/
453
454SUBMAINPROG
455 SUBPROG reset
456 init = 0
457
458 out 1 0
459 out 2 0
460 RF = 0
461 motor off
462 waitt 1000
463
464 if (brake==1 and get cannr==3) then
465 waitt 3000 /* wait 3s for DKC to stop the motor */
466 out (kIoModule+1) 0 /* brake the brake */
467 waitt 1000
468 endif
469
470 canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4)) (rf | (brake<<1))
471 return
472
473 /*----------------------------------*/
474 /* PROC_CANOPENMSG */
475 /*----------------------------------*/
476 SUBPROG PROC_SDOSET
477 idx = canhi&0xff00 | (canhi>>16)&0xff
478 subidx = canhi&0xff
479 sdoval = (canlo&0xff)<<24 | (canlo&0xff00)<<8 | (canlo>>8)&0xff00 | (canlo>>24)&0xff
480 canlo = 0
481/*
482 PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval
483*/
484 if (idx==0x1000 and subidx==1) then
485 if (sdoval==1) then
486 if (door==0) then
487 canlo = 1
488 else
489 preop = kFALSE
490 endif
491 elseif
492 preop = kTRUE
493 endif
494 elseif (idx==0x1003 and subidx==0 and sdoval==0) then
495 i = 9
496 while (i) do
497 errlist[i] = 0
498 i = i - 1
499 endwhile
500 elseif (idx==0x100c) then
501 guardtime = sdoval
502 ON PERIOD 0 GOSUB PROC_Timeout
503 if (lifetimefactor>0 and guardtime>0) then
504 timeouttime = TIME + guardtime*lifetimefactor
505 ON PERIOD guardtime GOSUB PROC_Timeout
506 endif
507 elseif (idx == 0x100d) then
508 lifetimefactor = sdoval
509 if (lifetimefactor==0) then
510 ON PERIOD 0 GOSUB PROC_Timeout
511 endif
512 elseif (idx == 0x1010 and sdoval == ('s'<<24|'a'<<16|'v'<<8|'e')) then
513 SAVEPROM
514 elseif (idx == 0x1800 and subidx == 1) then
515 ON PERIOD 0 GOSUB PROC_PDO1
516 if (sdoval>>31) then
517 pdo1on = kFALSE
518 else
519 ON PERIOD pdotime GOSUB PROC_PDO1
520 pdo1on = kTRUE
521 endif
522 elseif (idx == 0x2000) then
523 if (subidx == 0) then
524 SET POSERR sdoval
525 elseif (subidx == 1) then
526 SET NEGLIMIT sdoval
527 SET SWNEGLIMACT 1
528 elseif (subidx == 2) then
529 SET POSLIMIT sdoval
530 SET SWPOSLIMACT 1
531 elseif (subidx == 3) then
532 SET SWNEGLIMACT sdoval&1
533 SET SWPOSLIMACT (sdoval>>1)&1
534 endif
535 elseif (idx == 0x2002) then
536 VEL sdoval
537 elseif (idx == 0x2003) then
538 if (subidx) then
539 DEC sdoval
540 else
541 ACC sdoval
542 endif
543 elseif (idx == 0x3000) then
544 if (sdoval == ('o'<<24|'n'<<16)) then
545 MOTOR ON
546 elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then
547 MOTOR OFF
548 elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then
549 MOTOR STOP
550 endif
551 elseif (idx == 0x3001) then
552 if (sdoval == ('h'<<24|'o'<<16|'m'<<8|'e')) then
553 limitsw = GET I_POSLIMITSW
554 set I_POSLIMITSW 0
555 /* Disable Interrupt - same problem than with CANIN */
556 ON PERIOD 0 GOSUB PROC_PDO1
557 HOME
558 /* Reenable interrupt */
559 if (pdo1on) then
560 ON PERIOD pdotime GOSUB PROC_PDO1
561 endif
562 SET I_POSLIMITSW limitsw
563 endif
564 elseif (idx == 0x3002 and sdoval == ('o'<<24|'p'<<16|'e'<<8|'n')) then
565 sdoval=REOPEN 2 0
566 if (sdoval) then
567 PRINT "Error Reopen"
568 endif
569 elseif (idx == 0x3003 and sdoval == ('e'<<24|'x'<<16|'i'<<8|'t')) then
570 CANOUT sdotx (canhi&0xffffff | 0x60000000) 0
571 EXIT
572 elseif (idx == 0x3006) then
573 if (subidx == 0) then
574 if (sdoval == ('s'<<24|'t'<<16|'r'<<8|'t')) then
575 if (preop==kFALSE) then
576 CVEL 0
577 CSTART
578 else
579 canlo = 1
580 endif
581 elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then
582 CSTOP
583 endif
584 elseif (subidx == 1) then
585 /* It is unclear where this arbitraty factor comes from */
586 CVEL sdoval/4
587 endif
588 elseif (idx == 0x3007) then
589 if (preop==kFALSE) then
590 if (subidx==0 and sdoval == ('s'<<24|'y'<<16|'n'<<8|'c')) then
591 SYNCV
592 elseif (subidx==1 and sdoval == ('s'<<24|'y'<<16|'n'<<8|'c')) then
593 SYNCP
594 endif
595 else
596 canlo = 1
597 endif
598 elseif (idx == 0x3008) then
599 if (sdoval == ('o'<<24|'n'<<16)) then
600 NOWAIT ON
601 elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then
602 NOWAIT OFF
603 endif
604 elseif (idx == 0x4000) then
605 timeouttime = TIME + guardtime*lifetimefactor
606 elseif (idx == 0x6000) then
607 if (sdoval&1) then
608 SET POSDRCT -1
609 else
610 SET POSDRCT 1
611 endif
612 elseif (idx == 0x6002) then
613 SET VELRES sdoval
614 elseif (idx == 0x6003) then
615 if (subidx == 0 and sdoval == ('s'<<24|'e'<<16|'t'<<8)) then
616 DEF ORIGIN
617 elseif (subidx == 1) then
618 if (sdoval==0) then
619 RST ORIGIN
620 else
621 SET ORIGIN sdoval
622 endif
623 elseif (subidx == 2) then
624 SET HOME_OFFSET sdoval
625 endif
626 elseif (idx == 0x6004) then
627 if (preop==kFALSE) then
628 if (subidx==0) then
629 POSA sdoval
630 elseif (subidx==1) then
631 POSR sdoval
632 endif
633 else
634 canlo = 1
635 endif
636 elseif (idx == 0x6200) then
637 pdotime = sdoval
638 if (pdo1on) then
639 ON PERIOD 0 GOSUB PROC_PDO1
640 ON PERIOD pdotime GOSUB PROC_PDO1
641 endif
642 elseif (idx == 0x6501) then
643 SET ENCODER sdoval
644 elseif (idx == 0x6502) then
645 SET VELMAX sdoval
646 else
647 CANOUT sdotx (canhi&0xffffff | (1<<31)/*&0x80000000*/) 0
648 PRINT "Unknown SDO: idx=", idx, ", subidx=", subidx
649 goto ENDSDOSET
650 endif
651
652 if (pdo1on) then
653 GOSUB PROC_PDO1
654 endif
655
656 CANOUT sdotx (canhi&0xffffff | 0x60000000) canlo
657/*
658 PRINT "Sdo Set ", idx, "/", subidx
659*/
660 ENDSDOSET:
661 RETURN
662
663 /*----------------------------------*/
664
665 SUBPROG PROC_SDOREQ
666 idx = canhi&0xff00 | (canhi>>16)&0xff
667 subidx = canhi&0xff
668/*
669 PRINT "Requesting Idx:", idx, "/", subidx
670*/
671 if (idx == 0x1000 and subidx==1) then
672 sdoval = preop
673 elseif (idx == 0x1003) then
674 if (subidx >=0 and subidx<=9) then
675 sdoval = errlist[subidx-1]
676 endif
677 elseif (idx == 0x1004) then
678 if (subidx == 0) then
679 sdoval = 1
680 elseif (subidx == 1) then
681 sdoval = 0
682 elseif (subidx == 2) then
683 sdoval = 1
684 endif
685 elseif (idx == 0x1005) then
686 sdoval = 1<<31 | 0x80
687 elseif (idx == 0x100b) then
688 sdoval = nodenr
689 elseif (idx == 0x100a) then
690 sdoval = (kVERSION<<16) | kSUBVERSION
691 elseif (idx == 0x100c) then
692 sdoval = guardtime
693 elseif (idx == 0x100d) then
694 sdoval = lifetimefactor
695 elseif (idx == 0x100e) then
696 sdoval = 0x600|nodenr /*0x700 | nodenr*/
697 elseif (idx == 0x1010) then
698 sdoval = 1
699 elseif (idx == 0x1011) then
700 sdoval = 0
701 elseif (idx == 0x1014) then
702 sdoval = 0x80 | nodenr
703 elseif (idx == 0x1800) then
704 if (subidx == 1) then
705 sdoval = (~pdo1on)<<31 | (0x0180 + nodenr)
706 elseif (subidx == 2) then
707 sdoval = 0xfe
708 elseif (subidx == 3) then
709 sdoval = 0
710 endif
711 elseif (idx == 0x2000) then
712 if (subidx == 0) then
713 sdoval = GET POSERR
714 elseif (subidx == 1) then
715 sdoval = GET NEGLIMIT | (GET SWNEGLIMACT) << 30
716 elseif (subidx == 2) then
717 sdoval = GET POSLIMIT | (GET SWPOSLIMACT) << 31
718 elseif (subidx == 3) then
719 sdoval = (GET SWNEGLIMACT) | ((GET SWPOSLIMACT)<<1)
720 endif
721 elseif (idx == 0x2001) then
722 sdoval = AXEND
723 elseif (idx == 0x2002) then
724 sdoval = AVEL*4
725 elseif (idx == 0x2003) then
726 if (subidx==0) then
727 sdoval = INB 0
728 elseif (subidx>0 and subidx<2) then
729 sdoval = IN subidx
730 endif
731 elseif (idx == 0x2004) then
732 sdoval = STAT
733 elseif (idx == 0x6000) then
734 if (GET POSDRCT == 1) then
735 sdoval = 0
736 elseif (GET POSDRCT == -1) then
737 sdoval = 1
738 endif
739 elseif (idx == 0x6002) then
740 sdoval = GET VELRES
741 elseif (idx == 0x6003) then
742 sdoval = GET HOME_OFFSET
743 elseif (idx == 0x6004) then
744 if (subidx == 0) then
745 sdoval = APOS
746 elseif (subidx==1) then
747 sdoval = CPOS
748 endif
749 elseif (idx == 0x6501) then
750 sdoval = GET ENCODER
751 elseif (idx == 0x6502) then
752 sdoval = GET VELMAX
753 elseif (idx == 0x6508) thenä
754 sdoval = TIME
755 else
756 CANOUT sdotx (canhi&0xffffff | 0x80000000) 0
757 goto ENDSDOREQ
758 endif
759
760 canlo = (sdoval&0xff)<<24 | (sdoval&0xff00)<<8 | (sdoval>>8)&0xff00 | (sdoval>>24)&0xff
761 CANOUT sdotx (canhi&0xffffff | 0x43000000) canlo
762/*
763 PRINT "Returning: ", sdoval
764*/
765 ENDSDOREQ:
766 RETURN
767
768 /*----------------------------------------------------------------------*/
769 /* PROC_SDORX */
770 /*----------------------------------------------------------------------*/
771 /* This procedure handles incoming objects, it is called from the main */
772 /* loop. If the object ID (COB ID) identifies a SDO object, it is */
773 /* whether it is a object to set data (write into the object */
774 /* dictionary) or data is requested (read from object dictionary) */
775 /* If it isn't a valid SDO a error message is send. */
776 /* Remark: Only objects with the right node number are received by the */
777 /* main loop. */
778 /*----------------------------------------------------------------------*/
779
780 SUBPROG PROC_SDORX
781/* --Echo--
782 CANOUT sdotx canhi canlo
783*/
784 cmd = canhi>>24
785 if (cmd==0x23 OR cmd==0x2B OR cmd==0x2F) then
786 gosub PROC_SDOSET
787 elseif (cmd == 0x40) then
788 gosub PROC_SDOREQ
789 else
790 PRINT "Unknown SDO cmd", cmd
791 CANOUT sdotx (canhi&0xffffff | 0x80000000) 0
792 endif
793 ENDSDORX:
794 RETURN
795
796 /*----------------------------------*/
797 /* PROC_CANMSG */
798 /* called if a canmsg with */
799 /* cobid=2*CANNR+1 is received */
800 /* Warning: This doesn't fit to */
801 /* CanOpen specification */
802 /*----------------------------------*/
803 SUBPROG PROC_CANMSG
804 varnr = InMsg(-1)
805 PRINT "varnr=", varnr, "msgval=", msgval
806 RETURN
807
808/*-------------------------------------------------------------------------*/
809/* PDO 1 Interrupt */
810/*-------------------------------------------------------------------------*/
811 SUBPROG PROC_PDO1
812 CANOUT pdo1 (AXEND|(preop<<9)) APOS
813 RETURN
814
815/*-------------------------------------------------------------------------*/
816/* FENCE OPENED */
817/*-------------------------------------------------------------------------*/
818 SUBPROG DOOR_STATUS_CHANGED
819 preop = kTRUE
820 RETURN
821
822/*-------------------------------------------------------------------------*/
823/* Timeout Interrupt */
824/*-------------------------------------------------------------------------*/
825 SUBPROG PROC_Timeout
826 if (TIME > timeouttime) then
827 MOTOR STOP
828
829 if (firsttimeout==0) then
830
831 /* Tell the bus that an error occured */
832 CANOUT pdo2 0 0
833
834 i = errlist[1] + 1 /* Fill status of array */
835 while (i>2) do /* shift errors by one */
836 errlist[i] = errlist[i-1]
837 i = i - 1
838 endwhile /* set new errornumber */
839 errlist[2] = 100 /* User Error #100 */
840 if (errlist[1]<8) then /* write new size if enhanced */
841 errlist[1] = errlist[1] + 1
842 endif
843
844 errinf = 0
845
846 PRINT "User Timeout!"
847
848 /* tell the bus what exactly happened */
849 CANOUT pdo2 errlist[2] errinf
850 endif
851
852 firsttimeout = 1
853 else
854 firsttimeout = 0
855 endif
856 RETURN
857
858/*-------------------------------------------------------------------------*/
859/* Error sub proc */
860/*-------------------------------------------------------------------------*/
861 SUBPROG PROC_ERROR
862 /*MOTOR STOP*/
863 out 1 0
864 out 2 0
865 RF = 0
866
867 /* Tell the bus that an error occured */
868 CANOUT pdo2 0 0
869
870 waitt 100
871
872 if (brake==1 and get cannr==3) then
873 waitt 5000
874 out (kIoModule+1) 0
875 waitt 500
876 endif
877
878 print "Error #", errno
879
880 i = errlist[1] + 1 /* Fill status of array */
881 while (i>2) do /* shift errors by one */
882 errlist[i] = errlist[i-1]
883 i = i - 1
884 endwhile /* set new errornumber */
885 errlist[2] = ERRNO
886 if (errlist[1]<8) then /* write new size if enhanced */
887 errlist[1] = errlist[1] + 1
888 endif
889
890 errinf = 0
891
892 /* check if the error is repairable and repair */
893 if (errlist[2]==6) then
894 PRINT "No home forced!"
895 ERRCLR
896 elseif (errlist[2]==8) then
897 PRINT "Control deviation overflow."
898 ERRCLR
899 errinf = 0xaffe
900 elseif (errlist[2]==9) then
901 PRINT "Did'n find zero index."
902 ERRCLR
903 elseif (errlist[2]==11) then
904 poslsw = GET POSLIMIT
905 neglsw = GET NEGLIMIT
906 if ((GET SWNEGLIMACT) and APOS<=neglsw) then
907 PRINT "Negative software endswitch (", neglsw, ") activated at position ", APOS
908 SET SWNEGLIMACT 0
909 ERRCLR
910 CVEL (vres%100)
911 ACC (10*vres%100)
912 DEC (10*vres%100)
913 POSA neglsw + 100
914 SET SWNEGLIMACT 1
915 errinf = -1
916 elseif ((GET SWPOSLIMACT) and APOS>=poslsw) then
917 PRINT "Positive software endswitch (", poslsw, ") activated at position ", APOS
918 SET SWPOSLIMACT 0
919 ERRCLR
920 CVEL (vres%100) /* 1% */
921 ACC (10*vres%100) /* 10% */
922 DEC (10*vres%100) /* 10% */
923 POSA poslsw - 100
924 SET SWPOSLIMACT 1
925 errinf = 1
926 else
927 PRINT "Software endswitch activated - command skipped. Pos: ", APOS
928 ERRCLR
929 endif
930 elseif (errlist[2]==25) then
931 /* FIXME: To handle this correct you must make sure,
932 that the endswitch numbers are negative */
933 poslsw = -(GET I_POSLIMITSW)
934 neglsw = -(GET I_NEGLIMITSW)
935 if (IN poslsw == 0) then
936 PRINT "Positive endswitch activated at position ", APOS
937 SET I_POSLIMITSW 0
938 ERRCLR
939 CVEL (vres%100) /* 1% */
940 ACC (10*vres%100) /* 10% */
941 DEC (10*vres%100) /* 10% */
942 CSTART
943 WHILE (IN poslsw == 0) DO ENDWHILE
944 CSTOP
945 SET I_POSLIMITSW -poslsw
946 errinf = 1
947 elseif (IN neglsw == 0) then
948 PRINT "Negative endswitch activated at position ", APOS
949 SET I_NEGLIMITSW 0
950 ERRCLR
951 vres = GET VELRES
952 CVEL -(vres%100) /* 1% */
953 ACC (10*vres%100) /* 10% */
954 DEC (10*vres%100) /* 10% */
955 CSTART
956 WHILE (IN neglsw == 0) DO ENDWHILE
957 CSTOP
958 SET I_NEGLIMITSW -neglsw
959 errinf = -1
960 endif
961 elseif (errlist[2]==84) then
962 PRINT "Too many (>12) ON TIME interrupts."
963 ERRCLR
964 elseif (errlist[2]==89) then
965 PRINT "CAN I/O error."
966 errinf = REOPEN 1 0
967 ERRCLR
968 ELSE
969 PRINT "Error Function Called: ERRNO=", errlist[2]
970 endif
971
972 /* tell the bus what exactly happened */
973 CANOUT pdo2 errlist[2] errinf
974 RETURN
975
976/*-------------------------------------------------------------------------*/
977/* End of part for Programs called with GOSUB */
978/*-------------------------------------------------------------------------*/
979
980ENDPROG
Note: See TracBrowser for help on using the repository browser.