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

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