source: trunk/Cosy/aposs/Magic.m@ 11691

Last change on this file since 11691 was 8875, checked in by tbretz, 18 years ago
*** empty log message ***
File size: 40.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 = 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 100 /*950*/
219SET KDER 300 /*2000*/
220SET KINT 1000 /*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
331/*-------------------------------------------------------------------------*/
332/* Init */
333/*-------------------------------------------------------------------------*/
334/* Program Main Loop */
335/*-------------------------------------------------------------------------*/
336/* The main loop is the core of the program which handles incoming */
337/* objects. In principal CANIN should wait until an object is received, */
338/* but it stops waiting when an interrupt occurs. This is the reason why */
339/* the validity of the message must be checked. */
340/*-------------------------------------------------------------------------*/
341MAIN:
342 canhi = 0
343 canlo = 1
344
345 brake = 0
346 RF = 0
347
348 init = 0
349
350 oldstate = kTRUE
351
352 gosub reset
353
354 PRINT "Starting Mainloop..."
355
356 rc = 1
357 MAINLOOP:
358 if (rc==0) then /* It must be tested because ON PERIOD breaks 'wait for obj' */
359 gosub PROC_SDORX
360 else
361 fuse = in (kIoModule+1)
362 emcy = in (kIoModule+2)
363 vltg = in (kIoModule+3)
364 mode = in (kIoModule+4)
365 door = in (kIoModule+6)
366 ready = in 1
367
368 if (door==0) then
369 preop = kTRUE
370 endif
371
372 if (ready==0) and (RF==1) then
373 print "DKC not ready, but RF set... setting RF=AH=0!"
374 gosub reset
375 goto mainloop
376 elseif (mode==1) then
377 print "Control not in PC mode!"
378 gosub reset
379 SET PRGPAR 0
380 exit
381 elseif (fuse==0) then
382 print "Motor-Power Fuse not OK!"
383 gosub reset
384 goto mainloop
385 elseif (vltg==0) then
386 print "Overvoltage control broken!"
387 gosub reset
388 goto mainloop
389 elseif (emcy==0) then
390 print "Please release Emergency Stop!"
391 gosub reset
392 goto mainloop
393 elseif (ready==1) and (RF==0) then
394 print "DKC powered, RF=0... setting RF=AH=1!"
395 /*
396 * After switching on power wait at least 300ms until
397 * control changed state 'bb' to 'ab'
398 */
399 cvel 0
400 waitt 300
401 out 1 0
402 out 2 0
403 motor off
404 waitt 100
405 out 1 1
406 out 2 1
407 RF = 1
408 waitt 100
409
410 if (brake==0 and get cannr==3) then
411 out (kIoModule+1) 1
412 brake = 1
413 waitt 1000
414 endif
415
416 /* This "motor on" is necessary to make sure the floating analog output
417 doesn't move the telescope (slowly) */
418 motor on
419
420 canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4) | (rf<<5) | (brake<<6)) 0
421 /*
422 if (get cannr==2) then
423 syncv
424 print "Synchronizing speed..."
425 else
426 print "Starting CAN mode..." ^
427 goto CANSTART
428 endif */
429 elseif (ready==0) or (RF==0) then
430 print "No Power, no RF..."
431 canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4) | (rf<5) | (brake<<6)) 0
432 waitt 500
433 goto mainloop
434 endif
435 endif
436
437 if (preop==kTRUE and oldstate!=preop) then
438 print "Door switch changed state!"
439 gosub reset
440 endif
441 oldstate = preop
442
443 rc = CANIN sdorx -1 0 canhi canlo
444 goto mainloop
445
446ENDMAIN:
447 MOTOR STOP
448 MOTOR OFF
449 OUT 1 0
450 EXIT
451
452/*-------------------------------------------------------------------------*/
453/* Part for Programs called with GOSUB */
454/*-------------------------------------------------------------------------*/
455
456SUBMAINPROG
457 SUBPROG reset
458 init = 0
459
460 out 1 0
461 out 2 0
462 RF = 0
463 motor off
464 waitt 1000
465
466 if (brake==1 and get cannr==3) then
467 waitt 3000 /* wait 3s for DKC to stop the motor */
468 out (kIoModule+1) 0 /* brake the brake */
469 waitt 1000
470 endif
471
472 canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4)) (rf | (brake<<1))
473 return
474
475 /*----------------------------------*/
476 /* PROC_CANOPENMSG */
477 /*----------------------------------*/
478 SUBPROG PROC_SDOSET
479 idx = canhi&0xff00 | (canhi>>16)&0xff
480 subidx = canhi&0xff
481 sdoval = (canlo&0xff)<<24 | (canlo&0xff00)<<8 | (canlo>>8)&0xff00 | (canlo>>24)&0xff
482 canlo = 0
483/*
484 PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval
485*/
486 if (idx==0x1000 and subidx==1) then
487 preop = kTRUE
488 if (sdoval==1 and door==1) then
489 preop = kFALSE
490 else
491 canlo = 1
492 endif
493 elseif (idx==0x1003 and subidx==0 and sdoval==0) then
494 i = 9
495 while (i) do
496 errlist[i] = 0
497 i = i - 1
498 endwhile
499 elseif (idx==0x100c) then
500 guardtime = sdoval
501 ON PERIOD 0 GOSUB PROC_Timeout
502 if (lifetimefactor>0 and guardtime>0) then
503 timeouttime = TIME + guardtime*lifetimefactor
504 ON PERIOD guardtime GOSUB PROC_Timeout
505 endif
506 elseif (idx == 0x100d) then
507 lifetimefactor = sdoval
508 if (lifetimefactor==0) then
509 ON PERIOD 0 GOSUB PROC_Timeout
510 endif
511 elseif (idx == 0x1010 and sdoval == ('s'<<24|'a'<<16|'v'<<8|'e')) then
512 SAVEPROM
513 elseif (idx == 0x1800 and subidx == 1) then
514 ON PERIOD 0 GOSUB PROC_PDO1
515 if (sdoval>>31) then
516 pdo1on = kFALSE
517 else
518 ON PERIOD pdotime GOSUB PROC_PDO1
519 pdo1on = kTRUE
520 endif
521 elseif (idx == 0x2000) then
522 if (subidx == 0) then
523 SET POSERR sdoval
524 elseif (subidx == 1) then
525 SET NEGLIMIT sdoval
526 SET SWNEGLIMACT 1
527 elseif (subidx == 2) then
528 SET POSLIMIT sdoval
529 SET SWPOSLIMACT 1
530 elseif (subidx == 3) then
531 SET SWNEGLIMACT sdoval&1
532 SET SWPOSLIMACT (sdoval>>1)&1
533 endif
534 elseif (idx == 0x2002) then
535 VEL sdoval
536 elseif (idx == 0x2003) then
537 if (subidx) then
538 /* If velocity mode active */
539 /*if (AXEND&4)
540 DEC sdoval%4
541 else*/
542 DEC sdoval
543 /*endif*/
544 else
545 /*if (AXEND&4)
546 ACC sdoval%4
547 else*/
548 ACC sdoval
549 /*endif*/
550 endif
551 elseif (idx == 0x3000) then
552 if (sdoval == ('o'<<24|'n'<<16)) then
553 MOTOR ON
554 elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then
555 MOTOR OFF
556 elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then
557 MOTOR STOP
558 endif
559 elseif (idx == 0x3001) then
560 if (sdoval == ('h'<<24|'o'<<16|'m'<<8|'e')) then
561 limitsw = GET I_POSLIMITSW
562 set I_POSLIMITSW 0
563 /* Disable Interrupt - same problem than with CANIN */
564 ON PERIOD 0 GOSUB PROC_PDO1
565 HOME
566 /* Reenable interrupt */
567 if (pdo1on) then
568 ON PERIOD pdotime GOSUB PROC_PDO1
569 endif
570 SET I_POSLIMITSW limitsw
571 endif
572 elseif (idx == 0x3002 and sdoval == ('o'<<24|'p'<<16|'e'<<8|'n')) then
573 sdoval=REOPEN 2 0
574 if (sdoval) then
575 PRINT "Error Reopen"
576 endif
577 elseif (idx == 0x3003 and sdoval == ('e'<<24|'x'<<16|'i'<<8|'t')) then
578 CANOUT sdotx (canhi&0xffffff | 0x60000000) 0
579 EXIT
580 elseif (idx == 0x3006) then
581 if (subidx == 0) then
582 if (sdoval == ('s'<<24|'t'<<16|'r'<<8|'t')) then
583 if (preop==kFALSE) then
584 CVEL 0
585 CSTART
586 else
587 canlo = 1
588 endif
589 elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then
590 CSTOP
591 endif
592 elseif (subidx == 1) then
593 /* It is unclear where this arbitraty factor comes from */
594 CVEL sdoval%4
595 endif
596 elseif (idx == 0x3007) then
597 if (preop==kFALSE) then
598 if (subidx==0 and sdoval == ('s'<<24|'y'<<16|'n'<<8|'c')) then
599 SYNCV
600 elseif (subidx==1 and sdoval == ('s'<<24|'y'<<16|'n'<<8|'c')) then
601 SYNCP
602 endif
603 else
604 canlo = 1
605 endif
606 elseif (idx == 0x3008) then
607 if (sdoval == ('o'<<24|'n'<<16)) then
608 NOWAIT ON
609 elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then
610 NOWAIT OFF
611 endif
612 elseif (idx == 0x4000) then
613 timeouttime = TIME + guardtime*lifetimefactor
614 elseif (idx == 0x6000) then
615 if (sdoval&1) then
616 SET POSDRCT -1
617 else
618 SET POSDRCT 1
619 endif
620 elseif (idx == 0x6002) then
621 SET VELRES sdoval
622 elseif (idx == 0x6003) then
623 if (subidx == 0 and sdoval == ('s'<<24|'e'<<16|'t'<<8)) then
624 DEF ORIGIN
625 elseif (subidx == 1) then
626 if (sdoval==0) then
627 RST ORIGIN
628 else
629 SET ORIGIN sdoval
630 endif
631 elseif (subidx == 2) then
632 SET HOME_OFFSET sdoval
633 endif
634 elseif (idx == 0x6004) then
635 if (preop==kFALSE) then
636 if (subidx==0) then
637 POSA sdoval
638 elseif (subidx==1) then
639 POSR sdoval
640 endif
641 else
642 canlo = 1
643 endif
644 elseif (idx == 0x6200) then
645 pdotime = sdoval
646 if (pdo1on) then
647 ON PERIOD 0 GOSUB PROC_PDO1
648 ON PERIOD pdotime GOSUB PROC_PDO1
649 endif
650 elseif (idx == 0x6501) then
651 SET ENCODER sdoval
652 elseif (idx == 0x6502) then
653 SET VELMAX sdoval
654 else
655 CANOUT sdotx (canhi&0xffffff | (1<<31)/*&0x80000000*/) 0
656 PRINT "Unknown SDO: idx=", idx, ", subidx=", subidx
657 goto ENDSDOSET
658 endif
659
660 if (pdo1on) then
661 GOSUB PROC_PDO1
662 endif
663
664 CANOUT sdotx (canhi&0xffffff | 0x60000000) canlo
665/*
666 PRINT "Sdo Set ", idx, "/", subidx
667*/
668 ENDSDOSET:
669 RETURN
670
671 /*----------------------------------*/
672
673 SUBPROG PROC_SDOREQ
674 idx = canhi&0xff00 | (canhi>>16)&0xff
675 subidx = canhi&0xff
676/*
677 PRINT "Requesting Idx:", idx, "/", subidx
678*/
679 if (idx == 0x1000 and subidx==1) then
680 sdoval = preop
681 elseif (idx == 0x1003) then
682 if (subidx >=0 and subidx<=9) then
683 sdoval = errlist[subidx-1]
684 endif
685 elseif (idx == 0x1004) then
686 if (subidx == 0) then
687 sdoval = 1
688 elseif (subidx == 1) then
689 sdoval = 0
690 elseif (subidx == 2) then
691 sdoval = 1
692 endif
693 elseif (idx == 0x1005) then
694 sdoval = 1<<31 | 0x80
695 elseif (idx == 0x100b) then
696 sdoval = nodenr
697 elseif (idx == 0x100a) then
698 sdoval = (kVERSION<<16) | kSUBVERSION
699 elseif (idx == 0x100c) then
700 sdoval = guardtime
701 elseif (idx == 0x100d) then
702 sdoval = lifetimefactor
703 elseif (idx == 0x100e) then
704 sdoval = 0x600|nodenr /*0x700 | nodenr*/
705 elseif (idx == 0x1010) then
706 sdoval = 1
707 elseif (idx == 0x1011) then
708 sdoval = 0
709 elseif (idx == 0x1014) then
710 sdoval = 0x80 | nodenr
711 elseif (idx == 0x1800) then
712 if (subidx == 1) then
713 sdoval = (~pdo1on)<<31 | (0x0180 + nodenr)
714 elseif (subidx == 2) then
715 sdoval = 0xfe
716 elseif (subidx == 3) then
717 sdoval = 0
718 endif
719 elseif (idx == 0x2000) then
720 if (subidx == 0) then
721 sdoval = GET POSERR
722 elseif (subidx == 1) then
723 sdoval = GET NEGLIMIT | (GET SWNEGLIMACT) << 30
724 elseif (subidx == 2) then
725 sdoval = GET POSLIMIT | (GET SWPOSLIMACT) << 31
726 elseif (subidx == 3) then
727 sdoval = (GET SWNEGLIMACT) | ((GET SWPOSLIMACT)<<1)
728 endif
729 elseif (idx == 0x2001) then
730 sdoval = AXEND
731 elseif (idx == 0x2002) then
732 sdoval = AVEL*4
733 elseif (idx == 0x2003) then
734 if (subidx==0) then
735 sdoval = INB 0
736 elseif (subidx>0 and subidx<2) then
737 sdoval = IN subidx
738 endif
739 elseif (idx == 0x2004) then
740 sdoval = STAT
741 elseif (idx == 0x6000) then
742 if (GET POSDRCT == 1) then
743 sdoval = 0
744 elseif (GET POSDRCT == -1) then
745 sdoval = 1
746 endif
747 elseif (idx == 0x6002) then
748 sdoval = GET VELRES
749 elseif (idx == 0x6003) then
750 sdoval = GET HOME_OFFSET
751 elseif (idx == 0x6004) then
752 if (subidx == 0) then
753 sdoval = APOS
754 elseif (subidx==1) then
755 sdoval = CPOS
756 endif
757 elseif (idx == 0x6501) then
758 sdoval = GET ENCODER
759 elseif (idx == 0x6502) then
760 sdoval = GET VELMAX
761 elseif (idx == 0x6508) thenä
762 sdoval = TIME
763 else
764 CANOUT sdotx (canhi&0xffffff | 0x80000000) 0
765 goto ENDSDOREQ
766 endif
767
768 canlo = (sdoval&0xff)<<24 | (sdoval&0xff00)<<8 | (sdoval>>8)&0xff00 | (sdoval>>24)&0xff
769 CANOUT sdotx (canhi&0xffffff | 0x43000000) canlo
770/*
771 PRINT "Returning: ", sdoval
772*/
773 ENDSDOREQ:
774 RETURN
775
776 /*----------------------------------------------------------------------*/
777 /* PROC_SDORX */
778 /*----------------------------------------------------------------------*/
779 /* This procedure handles incoming objects, it is called from the main */
780 /* loop. If the object ID (COB ID) identifies a SDO object, it is */
781 /* whether it is a object to set data (write into the object */
782 /* dictionary) or data is requested (read from object dictionary) */
783 /* If it isn't a valid SDO a error message is send. */
784 /* Remark: Only objects with the right node number are received by the */
785 /* main loop. */
786 /*----------------------------------------------------------------------*/
787
788 SUBPROG PROC_SDORX
789/* --Echo--
790 CANOUT sdotx canhi canlo
791*/
792 cmd = canhi>>24
793 if (cmd==0x23 OR cmd==0x2B OR cmd==0x2F) then
794 gosub PROC_SDOSET
795 elseif (cmd == 0x40) then
796 gosub PROC_SDOREQ
797 else
798 PRINT "Unknown SDO cmd", cmd
799 CANOUT sdotx (canhi&0xffffff | 0x80000000) 0
800 endif
801 ENDSDORX:
802 RETURN
803
804 /*----------------------------------*/
805 /* PROC_CANMSG */
806 /* called if a canmsg with */
807 /* cobid=2*CANNR+1 is received */
808 /* Warning: This doesn't fit to */
809 /* CanOpen specification */
810 /*----------------------------------*/
811 SUBPROG PROC_CANMSG
812 varnr = InMsg(-1)
813 PRINT "varnr=", varnr, "msgval=", msgval
814 RETURN
815
816/*-------------------------------------------------------------------------*/
817/* PDO 1 Interrupt */
818/*-------------------------------------------------------------------------*/
819 SUBPROG PROC_PDO1
820 CANOUT pdo1 (AXEND|(((~preop)&1)<<8)) APOS
821 RETURN
822
823/*-------------------------------------------------------------------------*/
824/* FENCE OPENED */
825/*-------------------------------------------------------------------------*/
826 SUBPROG DOOR_STATUS_CHANGED
827 preop = kTRUE
828 RETURN
829
830/*-------------------------------------------------------------------------*/
831/* Timeout Interrupt */
832/*-------------------------------------------------------------------------*/
833 SUBPROG PROC_Timeout
834 if (TIME > timeouttime) then
835 MOTOR STOP
836
837 if (firsttimeout==0) then
838
839 /* Tell the bus that an error occured */
840 CANOUT pdo2 0 0
841
842 i = errlist[1] + 1 /* Fill status of array */
843 while (i>2) do /* shift errors by one */
844 errlist[i] = errlist[i-1]
845 i = i - 1
846 endwhile /* set new errornumber */
847 errlist[2] = 100 /* User Error #100 */
848 if (errlist[1]<8) then /* write new size if enhanced */
849 errlist[1] = errlist[1] + 1
850 endif
851
852 errinf = 0
853
854 PRINT "User Timeout!"
855
856 /* tell the bus what exactly happened */
857 CANOUT pdo2 errlist[2] errinf
858 endif
859
860 firsttimeout = 1
861 else
862 firsttimeout = 0
863 endif
864 RETURN
865
866/*-------------------------------------------------------------------------*/
867/* Error sub proc */
868/*-------------------------------------------------------------------------*/
869 SUBPROG PROC_ERROR
870 /*MOTOR STOP*/
871 out 1 0
872 out 2 0
873 RF = 0
874
875 /* Tell the bus that an error occured */
876 CANOUT pdo2 0 0
877
878 waitt 100
879
880 if (brake==1 and get cannr==3) then
881 waitt 5000
882 out (kIoModule+1) 0
883 waitt 500
884 endif
885
886 print "Error #", errno
887
888 i = errlist[1] + 1 /* Fill status of array */
889 while (i>2) do /* shift errors by one */
890 errlist[i] = errlist[i-1]
891 i = i - 1
892 endwhile /* set new errornumber */
893 errlist[2] = ERRNO
894 if (errlist[1]<8) then /* write new size if enhanced */
895 errlist[1] = errlist[1] + 1
896 endif
897
898 errinf = 0
899
900 /* check if the error is repairable and repair */
901 if (errlist[2]==6) then
902 PRINT "No home forced!"
903 ERRCLR
904 elseif (errlist[2]==8) then
905 PRINT "Control deviation overflow."
906 ERRCLR
907 errinf = 0xaffe
908 elseif (errlist[2]==9) then
909 PRINT "Did'n find zero index."
910 ERRCLR
911 elseif (errlist[2]==11) then
912 poslsw = GET POSLIMIT
913 neglsw = GET NEGLIMIT
914 if ((GET SWNEGLIMACT) and APOS<=neglsw) then
915 PRINT "Negative software endswitch (", neglsw, ") activated at position ", APOS
916 SET SWNEGLIMACT 0
917 ERRCLR
918 CVEL (vres%100)
919 ACC (10*vres%100)
920 DEC (10*vres%100)
921 POSA neglsw + 100
922 SET SWNEGLIMACT 1
923 errinf = -1
924 elseif ((GET SWPOSLIMACT) and APOS>=poslsw) then
925 PRINT "Positive software endswitch (", poslsw, ") activated at position ", APOS
926 SET SWPOSLIMACT 0
927 ERRCLR
928 CVEL (vres%100) /* 1% */
929 ACC (10*vres%100) /* 10% */
930 DEC (10*vres%100) /* 10% */
931 POSA poslsw - 100
932 SET SWPOSLIMACT 1
933 errinf = 1
934 else
935 PRINT "Software endswitch activated - command skipped. Pos: ", APOS
936 ERRCLR
937 endif
938 elseif (errlist[2]==25) then
939 /* FIXME: To handle this correct you must make sure,
940 that the endswitch numbers are negative */
941 poslsw = -(GET I_POSLIMITSW)
942 neglsw = -(GET I_NEGLIMITSW)
943 if (IN poslsw == 0) then
944 PRINT "Positive endswitch activated at position ", APOS
945 SET I_POSLIMITSW 0
946 ERRCLR
947 CVEL (vres%100) /* 1% */
948 ACC (10*vres%100) /* 10% */
949 DEC (10*vres%100) /* 10% */
950 CSTART
951 WHILE (IN poslsw == 0) DO ENDWHILE
952 CSTOP
953 SET I_POSLIMITSW -poslsw
954 errinf = 1
955 elseif (IN neglsw == 0) then
956 PRINT "Negative endswitch activated at position ", APOS
957 SET I_NEGLIMITSW 0
958 ERRCLR
959 vres = GET VELRES
960 CVEL -(vres%100) /* 1% */
961 ACC (10*vres%100) /* 10% */
962 DEC (10*vres%100) /* 10% */
963 CSTART
964 WHILE (IN neglsw == 0) DO ENDWHILE
965 CSTOP
966 SET I_NEGLIMITSW -neglsw
967 errinf = -1
968 endif
969 elseif (errlist[2]==84) then
970 PRINT "Too many (>12) ON TIME interrupts."
971 ERRCLR
972 elseif (errlist[2]==89) then
973 PRINT "CAN I/O error."
974 errinf = REOPEN 1 0
975 ERRCLR
976 ELSE
977 PRINT "Error Function Called: ERRNO=", errlist[2]
978 endif
979
980 /* tell the bus what exactly happened */
981 CANOUT pdo2 errlist[2] errinf
982 RETURN
983
984/*-------------------------------------------------------------------------*/
985/* End of part for Programs called with GOSUB */
986/*-------------------------------------------------------------------------*/
987
988ENDPROG
Note: See TracBrowser for help on using the repository browser.