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

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