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

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