Index: trunk/MagicSoft/Cosy/aposs/Magic.m =================================================================== --- trunk/MagicSoft/Cosy/aposs/Magic.m (revision 1806) +++ trunk/MagicSoft/Cosy/aposs/Magic.m (revision 1807) @@ -10,12 +10,29 @@ /* */ kVERSION = 0 /* */ -kSUBVERSION = 63 /* */ +kSUBVERSION = 67 /* */ /* */ /* HISTORY: */ +/* */ +/* * V0.67: */ +/* - trying to implement the control cabinet stuff */ +/* */ +/* * V0.66: */ +/* - implemented canopen node guarding (0x4000) */ +/* - implemented some kind of host guarding (in conjuction with */ +/* the implemented nodeguarding) */ +/* - removed old guarding using SDO 0x4000 */ +/* - implemented SDO 0x100c (guardtime) */ +/* - implemented SDO 0x100d (lifetimefactor) */ +/* */ +/* * V0.65: */ +/* - fixed a bug in the handling of the endswitches */ +/* */ +/* * V0.64: */ +/* - removed FORCEHOME */ /* */ /* * V0.63: */ /* - added movement handshake timeout (removed 0x400 WAITAX) */ /* - added brackets around string SDOs */ -/* - changed SDO 0x3007 to support both sync modes +/* - changed SDO 0x3007 to support both sync modes */ /* */ /* * V0.62: */ @@ -67,5 +84,5 @@ /* ----------------------------------------------------------------------- */ -PRINT "Magic Mics V", kVERSION, ".", kSUBVERSION /* */ +PRINT "Magic Mics V", kVERSION, ".", kSUBVERSION, " (CANbus controlled)" /**/ /* ----------------------------------------------------------------------- */ @@ -80,4 +97,6 @@ /* 0x100a x ro Software Version */ /* 0x100b x ro Node number */ +/* 0x100c x rw Guardtime (ms) */ +/* 0x100d x rw Lifetime factor */ /* 0x100e x ro COB ID for Guarding */ /* 0x1010 x wo Write data to EEPROM */ @@ -100,7 +119,5 @@ /* 1 wo Position Syncronisation 'sync' */ /* 0x3008 x wo Nowait 'on', 'off' */ -/* 0x4000 0 wo Reset timeout timer */ -/* 1 rw Enable timeout timer, 'on' (1), 'off'(0) */ -/* 2 rw Timeout timer time */ +/* 0x4000 0 wo Reset timeout timer (Nodeguard) */ /* 0x6000 x rw Rotation Direction */ /* 0x6002 x rw Velocity Resolution */ @@ -121,27 +138,23 @@ /* section for global constants */ /*-------------------------------------------------------------------------*/ -SET PRGPAR 0 /* Restart Program on Exit */ +SET PRGPAR -1 /* Don't restart any Program on Exit */ SET ENCODERTYPE 0 /* Incremental Encoder */ -SET ENCODER 500 /* Encoder has 500 Ticks */ SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ -SET MENCODER 500 /* Encoder has 500 Ticks (Master) */ -SET VELMAX 3600 /* Motor: Maximum rounds per minute */ -SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */ -SET ENDSWMOD 1 /* At End Switch Stop Motor with Max Decel. */ -SET ERRCOND 2 /* Motor Stop */ -SET POSDRCT 1 /* rotation direction */ + +SET ENDSWMOD 0 /* No End Switch */ +SET ERRCOND 2 /* Motor Stop, position control, no break */ +SET POSDRCT -1 /* rotation direction */ +/* OLD: SET POSDRCT -1 /* rotation direction */ SET POSFACT_Z 1 /* 1 user unit (be) = POSFACT_Z/POSFACT_N qc */ SET POSFACT_N 1 /* */ -SET HOME_FORCE 1 /* Force Home positioning on startup */ + +SET HOME_FORCE 0 /* Don't force Home positioning on mainloopup */ SET HOME_OFFSET 0 /* Offset between index and home position */ SET HOMETYPE 0 /* drive to home, reverse, go to next index */ -SET RAMPTYPE 0 /* Ramp Type: 0=Trapez, 1=Sinus */ /*----------------*/ /* syncronisation */ /*----------------*/ -SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ -SET MENCODER 500 /* Encoder has 500 Ticks (Master) */ SET SYNCFACTM 1 /* Master Sync Velocity factor */ SET SYNCFACTS 1 /* Slave Sync Velocity factor */ @@ -153,7 +166,7 @@ /* Inputs */ /*----------------*/ -SET I_REFSWITCH -2 /* Reference Switch, Input 2, leading edge */ -SET I_POSLIMITSW -2 /* Pos Limit Switch, Input 2, leading edge */ -SET I_NEGLIMITSW -1 /* Neg Limit Switch, Input 1, leading edge */ +SET I_REFSWITCH 0 /* Reference Switch */ +SET I_POSLIMITSW 0 /* Pos Limit Switch */ +SET I_NEGLIMITSW 0 /* Neg Limit Switch */ SET I_BREAK 0 /* Input which brakes a running program */ SET I_CONTINUE 0 /* Input to continue a broken program */ @@ -168,20 +181,57 @@ /*----------------*/ +/* Unit param. */ +/*----------------*/ +SET RAMPTYPE 1 /* Ramp Type: 0=Trapez, 1=Sinus */ +/* OLD: SET RAMPTYPE 0 /* Ramp Type: 0=Trapez, 1=Sinus */ +SET ENCODER 1500 /* Encoder has 1500 Ticks */ +SET MENCODER 1500 /* Encoder has 500 Ticks (Master) */ +SET VELMAX 3000 /* Motor: Maximum revolutions per minute */ +SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */ +SET RAMPMIN 10000 /* Shortest Ramp 10s */ + +/*----------------*/ /* Dflt vel & acc */ -/*----------------*/ +/*----------------*/ + +/* Prop=100, Div=300, Int=800 */ +if (get cannr==1) then +SET KPROP 100 +SET KDER 300 +SET KINT 1000 +elseif (get cannr==2) then +SET KPROP 100 +SET KDER 200 +SET KINT 150 +else +SET KPROP 350 +SET KDER 50 +SET KINT 350 +endif + vres = (GET ENCODER)*(GET VELMAX) /* ticks/R * R/M = ticks/min */ SET VELRES vres /* Set velocity units */ -SET HOME_VEL -(25*vres%100) /* Home position velocity: 25% */ -SET HOME_RAMP (25*vres%100) /* Home position accel: 25% */ -SET DFLTACC (10*vres%100) /* Default acceleratio: 10% */ -SET DFLTVEL (10*vres%100) /* Default velocity: 10% */ + + +/* OLD: SET HOME_VEL -(25*vres%100) /* Home position velocity: 25% */ +/* OLD: SET HOME_RAMP (25*vres%100) /* Home position accel: 25% */ /*----------------*/ -/* Software range */ +/* Manual control */ /*----------------*/ -SET SWPOSLIMACT 0 /* positive software limit switch inactive */ -SET SWNEGLIMACT 0 /* negative software limit switch inactive */ -SET POSLIMIT 0 /* positive software limit (qc) */ -SET NEGLIMIT 0 /* negative software limit (qc) */ +SET RAMPTYPE 1 /* Ramp: 0=linear, 1=sinus */ +defacc = 60*vres%100 + +SET DFLTACC defacc /* Default acceleratio: [%] */ +ACC defacc +DEC defacc*2 + /* Velocity which is reached in + a time given by RAMPMIN */ +SET DFLTVEL (1*vres%100) /* Default velocity [%] */ + +/*manvel = (4*vres%100) /* 150 U/min */ /* Max speed in man mode: [%] */ + +print "Vel Res (vel max): ", GET VELRES, " Encoder Ticks/min" +/*print "V_man: ", manvel, " Encoder Ticks/min"*/ /*-------------------------------------------------------------------------*/ @@ -194,7 +244,8 @@ pdo1on = kFALSE -timeout = 100 -timeouton = kFALSE -timeouttime = TIME +guardtime = 0 +lifetimefactor = 0 +timeouttime = TIME +firsttimeout = 0 /*-------------------------------------------------------------------------*/ @@ -203,21 +254,25 @@ /* The CAN Object are static object. This is why they must be deleted. */ /* The program should run in any of our nodes. */ -/* Therefor the standard CAN objects (SDO, PDO1, PDO2) for communication */ +/* Therefor the standard CAN objects (SDo, PDO1, PDO2) for communication */ /* are defined. The nodenumber is part of the object ID (this is somehow */ /* similar to the TCP/IP ports) */ /*-------------------------------------------------------------------------*/ -CANDEL -1 +CANDEL -1 + nodenr = GET CANNR PRINT "Initializing Node Nr.", nodenr -pdo1 = DEFCANOUT (0x180+nodenr) 8 -pdo2 = DEFCANOUT (0x280+nodenr) 8 -sdotx = DEFCANOUT (0x580+nodenr) 8 -sdorx = DEFCANIN (0x600+nodenr) 8 +pdo1 = DEFCANOUT (0x180+nodenr) 8 +pdo2 = DEFCANOUT (0x280+nodenr) 8 +pdo3 = DEFCANOUT (0x380+nodenr) 8 +sdotx = DEFCANOUT (0x580+nodenr) 8 +sdorx = DEFCANIN (0x600+nodenr) 8 +/* + guardrx = DEFCANIN (0x700+nodenr) 8 + guardtx = DEFCANOUT (0x700+nodenr) 8 + */ /* Close and reopen communication, enable buffering */ err = REOPEN 0 0 -/*-------------------------------------------------------------------------*/ -/* Init */ /*-------------------------------------------------------------------------*/ /* Before the motor control hardware is enabled (hi on output 1) */ @@ -226,9 +281,10 @@ /* of the program. */ /*-------------------------------------------------------------------------*/ -MOTOR STOP +/*MOTOR STOP MOTOR OFF CVEL 0 +OUT 1 1 +*/ NOWAIT OFF -OUT 1 1 /*-------------------------------------------------------------------------*/ @@ -249,4 +305,9 @@ ON ERROR GOSUB PROC_ERROR +/***************************************************************************/ +kIoModule = 4*256 + +/*-------------------------------------------------------------------------*/ +/* Init */ /*-------------------------------------------------------------------------*/ /* Program Main Loop */ @@ -261,12 +322,90 @@ canlo = 1 + brake = 0 + RF = 0 + + init = 0 + + gosub reset + PRINT "Starting Mainloop..." - + + rc = 1 MAINLOOP: - rc = CANIN sdorx 0 0 canhi canlo if (rc==0) then /* It must be tested because ON PERIOD breaks 'wait for obj' */ gosub PROC_SDORX + else + fuse = in (kIoModule+1) + emcy = in (kIoModule+2) + vltg = in (kIoModule+3) + mode = in (kIoModule+4) + ready = in 1 + + if (ready==0) and (RF==1) then + print "DKC not ready, but RF set... setting RF=AH=0!" + gosub reset + goto mainloop + elseif mode==1 then + print "Control not in PC mode!" + gosub reset + SET PRGPAR 0 + exit + elseif fuse==0 then + print "Motor-Power Fuse not OK!" + gosub reset + goto mainloop + elseif vltg==0 then + print "Overvoltage control broken!" + gosub reset + goto mainloop + elseif emcy==0 then + print "Please release Emergency Stop!" + gosub reset + goto mainloop + elseif (ready==1) and (RF==0) then + print "DKC powered, RF=0... setting RF=AH=1!" + /* + * After switching on power wait at least 300ms until + * control changed state 'bb' to 'ab' + */ + cvel 0 + waitt 300 + out 1 0 + out 2 0 + motor off + waitt 100 + out 1 1 + out 2 1 + RF = 1 + waitt 100 + + if (brake==0 and get cannr==3) then + out (kIoModule+1) 1 + brake = 1 + waitt 1000 + endif + + motor on + + canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4) | (rf<<5) | (brake<<6)) 0 + /* + if (get cannr==2) then + syncv + print "Synchronizing speed..." + else + print "Starting CAN mode..." ^ + goto CANSTART + endif */ + elseif (ready==0) or (RF==0) then + print "No Power, no RF..." + canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4) | (rf<5) | (brake<<6)) 0 + waitt 500 + goto mainloop + endif endif - goto MAINLOOP + + rc = CANIN sdorx -1 0 canhi canlo + goto mainloop + ENDMAIN: MOTOR STOP @@ -280,4 +419,22 @@ SUBMAINPROG + SUBPROG reset + init = 0 + + out 1 0 + out 2 0 + RF = 0 + motor off + waitt 1000 + + if (brake==1 and get cannr==3) then + waitt 3000 /* wait 3s for DKC to stop the motor */ + out (kIoModule+1) 0 /* brake the brake */ + waitt 1000 + endif + + canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4)) (rf | (brake<<1)) + return + /*----------------------------------*/ /* PROC_CANOPENMSG */ @@ -290,5 +447,5 @@ PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval */ - if (idx == 0x1003 and subidx == 0 and sdoval == 0) then + if (idx==0x1003 and subidx==0 and sdoval==0) then i = 9 while (i) do @@ -296,4 +453,16 @@ i = i - 1 endwhile + elseif (idx==0x100c) then + guardtime = sdoval + ON PERIOD 0 GOSUB PROC_Timeout + if (lifetimefactor>0 and guardtime>0) then + timeouttime = TIME + guardtime*lifetimefactor + ON PERIOD guardtime GOSUB PROC_Timeout + endif + elseif (idx == 0x100d) then + lifetimefactor = sdoval + if (lifetimefactor==0) then + ON PERIOD 0 GOSUB PROC_Timeout + endif elseif (idx == 0x1010 and sdoval == ('s'<<24|'a'<<16|'v'<<8|'e')) then SAVEPROM @@ -380,31 +549,6 @@ endif elseif (idx == 0x4000) then - if (subidx == 0) then - timeouttime = TIME + timeout - elseif (subidx == 1) then - ON PERIOD 0 GOSUB PROC_Timeout - if (sdoval == ('o'<<24|'n'<<16)) then - timeouttime = TIME + timeout - ON PERIOD timeout GOSUB PROC_Timeout - timeouton = kTRUE - elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then - timeouton = kFALSE - endif - elseif (subidx == 2) then - timeout = sdoval - if (timeouton) then - ON PERIOD 0 GOSUB PROC_Timeout - timeouttime = TIME + timeout - ON PERIOD timeout GOSUB PROC_Timeout - endif - endif -/* elseif (idx == 0x4000 and - (sdoval>>24)&0xff == 'S' and - (sdoval>>16)&0xff == 'T' and - (sdoval>>8) &0xff == 'O' and - (sdoval) &0xff == 'P') then - CANOUT sdotx (canhi&0xffffff | 0x60000000) 0 - goto ENDMAIN -*/ elseif (idx == 0x6000) then + timeouttime = TIME + guardtime*lifetimefactor + elseif (idx == 0x6000) then if (sdoval&1) then SET POSDRCT -1 @@ -485,6 +629,10 @@ elseif (idx == 0x100a) then sdoval = (kVERSION<<16) | kSUBVERSION + elseif (idx == 0x100c) then + sdoval = guardtime + elseif (idx == 0x100d) then + sdoval = lifetimefactor elseif (idx == 0x100e) then - sdoval = 0x700 | nodenr + sdoval = 0x600|nodenr /*0x700 | nodenr*/ elseif (idx == 0x1010) then sdoval = 1 @@ -523,10 +671,4 @@ elseif (idx == 0x2004) then sdoval = STAT - elseif (idx == 0x4000) then - if (subidx == 1) then - sdoval = timeouton - elseif (subidx == 2) then - sdoval = timeout - endif elseif (idx == 0x6000) then if (GET POSDRCT == 1) then @@ -614,9 +756,10 @@ /* Timeout Interrupt */ /*-------------------------------------------------------------------------*/ - SUBPROG PROC_Timeout - diff = timeouttime - TIME - if (diff < 0) then + SUBPROG PROC_Timeout + if (TIME > timeouttime) then MOTOR STOP - + + if (firsttimeout==0) then + /* Tell the bus that an error occured */ CANOUT pdo2 0 0 @@ -633,13 +776,14 @@ errinf = 0 - - /*if (firstcall)*/ - PRINT "User Timeout at uptime=", (TIME%1000), "s missed=", -diff, "ms" - /*firstcall = kFALSE */ + + PRINT "User Timeout!" /* tell the bus what exactly happened */ CANOUT pdo2 errlist[2] errinf - /*elseif - firstcall = kTRUE*/ + endif + + firsttimeout = 1 + else + firsttimeout = 0 endif RETURN @@ -649,9 +793,22 @@ /*-------------------------------------------------------------------------*/ SUBPROG PROC_ERROR - MOTOR STOP + /*MOTOR STOP*/ + out 1 0 + out 2 0 + RF = 0 /* Tell the bus that an error occured */ CANOUT pdo2 0 0 + + waitt 100 + if (brake==1 and get cannr==3) then + waitt 5000 + out (kIoModule+1) 0 + waitt 500 + endif + + print "Error #", errno + i = errlist[1] + 1 /* Fill status of array */ while (i>2) do /* shift errors by one */ @@ -693,5 +850,5 @@ PRINT "Positive software endswitch (", poslsw, ") activated at position ", APOS SET SWPOSLIMACT 0 - ERRCLR + ERRCLR CVEL (vres%100) /* 1% */ ACC (10*vres%100) /* 10% */ @@ -730,7 +887,7 @@ DEC (10*vres%100) /* 10% */ CSTART - WHILE (IN poslsw == 0) DO ENDWHILE + WHILE (IN neglsw == 0) DO ENDWHILE CSTOP - SET I_NEGLIMITSW -poslsw + SET I_NEGLIMITSW -neglsw errinf = -1 endif Index: trunk/MagicSoft/Cosy/aposs/Manual.m =================================================================== --- trunk/MagicSoft/Cosy/aposs/Manual.m (revision 1806) +++ trunk/MagicSoft/Cosy/aposs/Manual.m (revision 1807) @@ -4,7 +4,19 @@ /* */ kVERSION = 0 /* */ -kSUBVERSION = 3 /* */ +kSUBVERSION = 6 /* */ /* */ /* HISTORY: */ +/* */ +/* * V0.6 */ +/* - do not reset the velocity if set already */ +/* - set acceleration to 20% */ +/* */ +/* * V0.5: */ +/* - changes 'DKC Ready' signal to IN1 for all MACS */ +/* - control brake only when cannr==3 */ +/* */ +/* * V0.4: */ +/* - restart MACS with PRGPAR 1 when not in manual mode */ +/* - replaced label reset by a subprg */ /* */ /* * V0.3: */ @@ -112,5 +124,5 @@ /*----------------*/ SET RAMPTYPE 1 /* Ramp: 0=linear, 1=sinus */ -defacc = 60*vres%100 +defacc = 10*vres%100 SET DFLTACC defacc /* Default acceleratio: [%] */ @@ -124,5 +136,5 @@ print "Vel Res (vel max): ", GET VELRES, " Encoder Ticks/min" -print "V_man: ", manvel, " Encoder Ticks/min" +print "V_man: ", manvel, " Encoder Ticks/min, Acc=", defacc /*----------------*/ @@ -152,17 +164,8 @@ brake = 0 +RF = 0 +velo = 0 -reset: - out 1 0 - out 2 0 - RF = 0 - motor off - waitt 1000 - - if (brake==1) then - waitt 3000 /* wait 3s for DKC to stop the motor */ - out (kIoModule+1) 0 /* brake the brake */ - waitt 1000 - endif +gosub reset mainloop: @@ -171,26 +174,35 @@ vltg = in (kIoModule+3) mode = in (kIoModule+4) - + /* if (get cannr==1) or (get cannr==2) then - ready = in 1 + */ + ready = in 1 + /* elseif (get cannr==3) then - ready = in (kIoModule+5) /* !!!FIXME!!! */ + ready = in (kIoModule+5) endif + */ if (ready==0) and (RF==1) then print "DKC not ready, but RF set... setting RF=AH=0!" - goto reset + gosub reset + goto mainloop + elseif mode==0 then + print "Control not in manual mode!" + gosub reset + SET PRGPAR 1 + exit elseif fuse==0 then print "Motor-Power Fuse not OK!" - goto reset + gosub reset + goto mainloop elseif vltg==0 then print "Overvoltage control broken!" - goto reset + gosub reset + goto mainloop elseif emcy==0 then print "Please release Emergency Stop!" - goto reset - elseif mode==0 then - print "Control not in manual mode!" - goto reset + gosub reset + goto mainloop elseif (ready==1) and (RF==0) then print "DKC powered, RF=0... setting RF=AH=1!" @@ -210,5 +222,5 @@ waitt 100 - if (brake==0) then + if (brake==0 and get cannr==3) then out (kIoModule+1) 1 brake = 1 @@ -239,14 +251,32 @@ backward = in 3 - if (forward==1) and (backward==0) then - cvel manvel - elseif (forward==0) and (backward==1) then + if (forward==1) and (backward==0) and (velo!=manvel) then + cvel manvel + velo = manvel + elseif (forward==0) and (backward==1) and (velo!=-manvel) then cvel -manvel - else - cvel 0 + velo = -manvel + elseif (forward==backward) and (velo!=0) then + cvel 0 + velo = 0 endif goto mainloop SUBMAINPROG + subprog reset + out 1 0 + out 2 0 + RF = 0 + motor off + velo = 0 + waitt 1000 + + if (brake==1 and get cannr==3) then + waitt 3000 /* wait 3s for DKC to stop the motor */ + out (kIoModule+1) 0 /* brake the brake */ + waitt 1000 + endif + return + subprog suberror out 1 0 @@ -254,6 +284,7 @@ RF = 0 waitt 100 + velo = 0 - if (brake==1) then + if (brake==1 and get cannr==3) then waitt 5000 out (kIoModule+1) 0 Index: trunk/MagicSoft/Cosy/aposs/Model.m =================================================================== --- trunk/MagicSoft/Cosy/aposs/Model.m (revision 1807) +++ trunk/MagicSoft/Cosy/aposs/Model.m (revision 1807) @@ -0,0 +1,758 @@ +/*-------------------------------------------------------------------------*/ +/* DIM section for Array definition (arrays are globals) */ +/*-------------------------------------------------------------------------*/ + +DIM errlist[9] /* idx 1=number of valid entries */ + +/* ----------------------------------------------------------------------- */ +/* */ +/* Version: */ +/* */ +kVERSION = 0 /* */ +kSUBVERSION = 66 /* */ +/* */ +/* HISTORY: */ +/* */ +/* * V0.66: */ +/* - implemented canopen node guarding (0x4000) */ +/* - implemented some kind of host guarding (in conjuction with */ +/* the implemented nodeguarding) */ +/* - removed old guarding using SDO 0x4000 */ +/* - implemented SDO 0x100c (guardtime) */ +/* - implemented SDO 0x100d (lifetimefactor) */ +/* */ +/* * V0.65: */ +/* - fixed a bug in the handling of the endswitches */ +/* */ +/* * V0.64: */ +/* - removed FORCEHOME */ +/* */ +/* * V0.63: */ +/* - added movement handshake timeout (removed 0x400 WAITAX) */ +/* - added brackets around string SDOs */ +/* - changed SDO 0x3007 to support both sync modes */ +/* */ +/* * V0.62: */ +/* - changed handling of 0x2000/1/2 added /3 */ +/* */ +/* * V0.61: */ +/* - corrected problems with the error handling */ +/* */ +/* * V0.60: */ +/* - introduced syncronisation */ +/* */ +/* * V0.52: */ +/* - changed the handling of the endswitch error (unknown switch) */ +/* */ +/* * V0.51: */ +/* - made errlist working */ +/* */ +/* * V0.50: */ +/* - Rearanged many object numbers */ +/* - Added object dictonary to comments */ +/* */ +/* * V0.43: */ +/* - Added Object 0x1003, 0x1004, 0x1005 */ +/* */ +/* * V0.42: */ +/* - Added APOS to PDO1 */ +/* */ +/* * V0.41: */ +/* - Period Interrupt diabled while HOME */ +/* */ +/* * V0.40: */ +/* - Introduced object 0x1010, 0x1011 */ +/* */ +/* * V0.38: */ +/* - Introduced object 0x100a */ +/* */ +/* * V0.37: */ +/* - ON ERROR GOSUB moved after new init section */ +/* */ +/* * V0.36: */ +/* - Enahnced Initialization (NOWAIT OFF, etc.) */ +/* */ +/* * V0.35: */ +/* - SDO 0x4003/EXIT introduced */ +/* */ +/* * V0.34: */ +/* - PDO1 as answer to a SDO added (maybe SDO changes state) */ +/* */ +/* ----------------------------------------------------------------------- */ + +PRINT "Magic Mics V", kVERSION, ".", kSUBVERSION /* */ + +/* ----------------------------------------------------------------------- */ +/* */ +/* Object Dictionary: */ +/* */ +/* 0x1003 x rw Read delete error list (subidx 0-9) */ +/* 0x1004 0 ro Nr of PDOs (transmit) */ +/* 1 ro Nr of PDOs (synchron) */ +/* 2 ro Nr of PDOs (asynchron) */ +/* 0x1005 x ro COB ID for Syncs */ +/* 0x100a x ro Software Version */ +/* 0x100b x ro Node number */ +/* 0x100c x rw Guardtime (ms) */ +/* 0x100d x rw Lifetime factor */ +/* 0x100e x ro COB ID for Guarding */ +/* 0x1010 x wo Write data to EEPROM */ +/* 0x1014 x ro COB ID for Emergency */ +/* 0x1800 x rw Enable PDO1 (Axe Status, Position) */ +/* 0x2000 0 rw Maximum positioning error */ +/* 1 rw Negative Software Endswitch Value (Set=Enable) */ +/* 2 rw Positive Software Endswitch Value (Set=Enable) */ +/* 3 rw Enable/Disable Software Endswitch */ +/* 0x2002 x rw Velocity */ +/* 0x2003 0 wo Acceleration */ +/* 1 wo Deceleration */ +/* 0x3000 x wo Motor 'on', 'off', 'stop' */ +/* 0x3001 x wo Home 'home' */ +/* 0x3002 x wo Reopen Communication 'open' */ +/* 0x3003 x wo Exit Program 'exit' */ +/* 0x3006 0 wo Velocity Mode 'strt', 'stop' */ +/* 1 wo VelMode Velocity */ +/* 0x3007 0 wo Velocity Syncronisation 'sync' */ +/* 1 wo Position Syncronisation 'sync' */ +/* 0x3008 x wo Nowait 'on', 'off' */ +/* 0x4000 0 wo Reset timeout timer (Nodeguard) */ +/* 0x6000 x rw Rotation Direction */ +/* 0x6002 x rw Velocity Resolution */ +/* 0x6003 0 wo Define present position as origin ('set') */ +/* 1 wo Define new origin (0=delete) */ +/* 2 rw Home Offset */ +/* 0x6004 0 rw Absolute Position */ +/* 1 wo Relative Position */ +/* 1 ro Control Position */ +/* 0x6501 x rw Encoder Resolution */ +/* 0x6502 x rw Maximum Velocity */ +/* 0x6508 x ro Time since switch on */ +/* */ +/* ----------------------------------------------------------------------- */ + + +/*-------------------------------------------------------------------------*/ +/* section for global constants */ +/*-------------------------------------------------------------------------*/ +SET PRGPAR 0 /* Restart Program on Exit */ + +SET ENCODERTYPE 0 /* Incremental Encoder */ +SET ENCODER 500 /* Encoder has 500 Ticks */ +SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ +SET MENCODER 500 /* Encoder has 500 Ticks (Master) */ +SET VELMAX 3600 /* Motor: Maximum rounds per minute */ +SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */ +SET ENDSWMOD 1 /* At End Switch Stop Motor with Max Decel. */ +SET ERRCOND 2 /* Motor Stop */ +SET POSDRCT 1 /* rotation direction */ +SET POSFACT_Z 1 /* 1 user unit (be) = POSFACT_Z/POSFACT_N qc */ +SET POSFACT_N 1 /* */ +SET HOME_FORCE 0 /* Force Home positioning on startup */ +SET HOME_OFFSET 0 /* Offset between index and home position */ +SET HOMETYPE 0 /* drive to home, reverse, go to next index */ +SET RAMPTYPE 0 /* Ramp Type: 0=Trapez, 1=Sinus */ + +/*----------------*/ +/* syncronisation */ +/*----------------*/ +SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ +SET MENCODER 500 /* Encoder has 500 Ticks (Master) */ +SET SYNCFACTM 1 /* Master Sync Velocity factor */ +SET SYNCFACTS 1 /* Slave Sync Velocity factor */ +SET SYNCPOSOFFS 0 /* Sync Position offset between M/S */ +SET SYNCACCURACY 50 /* When to set Accuracy Flag */ +SET REVERS 0 /* How to handle reversation of vel */ + +/*----------------*/ +/* Inputs */ +/*----------------*/ +SET I_REFSWITCH -2 /* Reference Switch, Input 2, leading edge */ +SET I_POSLIMITSW -2 /* Pos Limit Switch, Input 2, leading edge */ +SET I_NEGLIMITSW -1 /* Neg Limit Switch, Input 1, leading edge */ +SET I_BREAK 0 /* Input which brakes a running program */ +SET I_CONTINUE 0 /* Input to continue a broken program */ +SET I_ERRCLR 0 /* Input to clear error */ + +/*----------------*/ +/* Outputs */ +/*----------------*/ +SET O_AXMOVE 0 /* Motor control is working */ +SET O_BRAKE 0 /* Brake */ +SET O_ERROR 0 /* error occured */ + +/*----------------*/ +/* Dflt vel & acc */ +/*----------------*/ +vres = (GET ENCODER)*(GET VELMAX) /* ticks/R * R/M = ticks/min */ +SET VELRES vres /* Set velocity units */ +SET HOME_VEL -(25*vres%100) /* Home position velocity: 25% */ +SET HOME_RAMP (25*vres%100) /* Home position accel: 25% */ +SET DFLTACC (10*vres%100) /* Default acceleratio: 10% */ +SET DFLTVEL (10*vres%100) /* Default velocity: 10% */ + +/*----------------*/ +/* Software range */ +/*----------------*/ +SET SWPOSLIMACT 0 /* positive software limit switch inactive */ +SET SWNEGLIMACT 0 /* negative software limit switch inactive */ +SET POSLIMIT 0 /* positive software limit (qc) */ +SET NEGLIMIT 0 /* negative software limit (qc) */ + +/*-------------------------------------------------------------------------*/ +/* const section for constant velues */ +/*-------------------------------------------------------------------------*/ +kTRUE = 1 +kFALSE = 0 + +pdotime = 100 +pdo1on = kFALSE + +guardtime = 0 +lifetimefactor = 0 +timeouttime = TIME +firsttimeout = 0 + +/*-------------------------------------------------------------------------*/ +/* Can Open Definitions */ +/*-------------------------------------------------------------------------*/ +/* The CAN Object are static object. This is why they must be deleted. */ +/* The program should run in any of our nodes. */ +/* Therefor the standard CAN objects (SDo, PDO1, PDO2) for communication */ +/* are defined. The nodenumber is part of the object ID (this is somehow */ +/* similar to the TCP/IP ports) */ +/*-------------------------------------------------------------------------*/ +CANDEL -1 +nodenr = GET CANNR +PRINT "Initializing Node Nr.", nodenr +pdo1 = DEFCANOUT (0x180+nodenr) 8 +pdo2 = DEFCANOUT (0x280+nodenr) 8 +sdotx = DEFCANOUT (0x580+nodenr) 8 +sdorx = DEFCANIN (0x600+nodenr) 8 +/* + guardrx = DEFCANIN (0x700+nodenr) 8 + guardtx = DEFCANOUT (0x700+nodenr) 8 + */ + +/* Close and reopen communication, enable buffering */ +err = REOPEN 0 0 + +/*-------------------------------------------------------------------------*/ +/* Init */ +/*-------------------------------------------------------------------------*/ +/* Before the motor control hardware is enabled (hi on output 1) */ +/* the commands make sure, that the motor will not start moving. */ +/* As default positioning commands doesn't stop the further execution */ +/* of the program. */ +/*-------------------------------------------------------------------------*/ +MOTOR STOP +MOTOR OFF +CVEL 0 +NOWAIT OFF +OUT 1 1 + +/*-------------------------------------------------------------------------*/ +/* ON ... GOSUB ... definitions */ +/*-------------------------------------------------------------------------*/ +/* The errorlist one can retreive using the corresponding CAN object */ +/* should be emty when the node is initialized (arrays are static objects) */ +/* therefor it must be deleted. */ +/* Errors are handled in an interrupt procedure called PROC_ERROR */ +/*-------------------------------------------------------------------------*/ +/* ON CANMSG GOSUB PROC_CANMSG */ +i = 9 +while (i) do + errlist[i] = 0 + i = i - 1 +endwhile + +ON ERROR GOSUB PROC_ERROR + +/*-------------------------------------------------------------------------*/ +/* Program Main Loop */ +/*-------------------------------------------------------------------------*/ +/* The main loop is the core of the program which handles incoming */ +/* objects. In principal CANIN should wait until an object is received, */ +/* but it stops waiting when an interrupt occurs. This is the reason why */ +/* the validity of the message must be checked. */ +/*-------------------------------------------------------------------------*/ +MAIN: + canhi = 0 + canlo = 1 + + PRINT "Starting Mainloop..." + + MAINLOOP: + rc = CANIN sdorx 0 0 canhi canlo + if (rc==0) then /* It must be tested because ON PERIOD breaks 'wait for obj' */ + gosub PROC_SDORX + endif + goto MAINLOOP +ENDMAIN: + MOTOR STOP + MOTOR OFF + OUT 1 0 + EXIT + +/*-------------------------------------------------------------------------*/ +/* Part for Programs called with GOSUB */ +/*-------------------------------------------------------------------------*/ + +SUBMAINPROG + /*----------------------------------*/ + /* PROC_CANOPENMSG */ + /*----------------------------------*/ + SUBPROG PROC_SDOSET + idx = canhi&0xff00 | (canhi>>16)&0xff + subidx = canhi&0xff + sdoval = (canlo&0xff)<<24 | (canlo&0xff00)<<8 | (canlo>>8)&0xff00 | (canlo>>24)&0xff +/* + PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval +*/ + if (idx==0x1003 and subidx==0 and sdoval==0) then + i = 9 + while (i) do + errlist[i] = 0 + i = i - 1 + endwhile + elseif (idx==0x100c) then + guardtime = sdoval + ON PERIOD 0 GOSUB PROC_Timeout + if (lifetimefactor>0 and guardtime>0) then + timeouttime = TIME + guardtime*lifetimefactor + ON PERIOD guardtime GOSUB PROC_Timeout + endif + elseif (idx == 0x100d) then + lifetimefactor = sdoval + if (lifetimefactor==0) then + ON PERIOD 0 GOSUB PROC_Timeout + endif + elseif (idx == 0x1010 and sdoval == ('s'<<24|'a'<<16|'v'<<8|'e')) then + SAVEPROM + elseif (idx == 0x1800 and subidx == 1) then + ON PERIOD 0 GOSUB PROC_PDO1 + if (sdoval>>31) then + pdo1on = kFALSE + else + ON PERIOD pdotime GOSUB PROC_PDO1 + pdo1on = kTRUE + endif + elseif (idx == 0x2000) then + if (subidx == 0) then + SET POSERR sdoval + elseif (subidx == 1) then + SET NEGLIMIT sdoval + SET SWNEGLIMACT 1 + elseif (subidx == 2) then + SET POSLIMIT sdoval + SET SWPOSLIMACT 1 + elseif (subidx == 3) then + SET SWNEGLIMACT sdoval&1 + SET SWPOSLIMACT (sdoval>>1)&1 + endif + elseif (idx == 0x2002) then + VEL sdoval + elseif (idx == 0x2003) then + if (subidx) then + DEC sdoval + else + ACC sdoval + endif + elseif (idx == 0x3000) then + if (sdoval == ('o'<<24|'n'<<16)) then + MOTOR ON + elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then + MOTOR OFF + elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then + MOTOR STOP + endif + elseif (idx == 0x3001) then + if (sdoval == ('h'<<24|'o'<<16|'m'<<8|'e')) then + limitsw = GET I_POSLIMITSW + set I_POSLIMITSW 0 + /* Disable Interrupt - same problem than with CANIN */ + ON PERIOD 0 GOSUB PROC_PDO1 + HOME + /* Reenable interrupt */ + if (pdo1on) then + ON PERIOD pdotime GOSUB PROC_PDO1 + endif + SET I_POSLIMITSW limitsw + endif + elseif (idx == 0x3002 and sdoval == ('o'<<24|'p'<<16|'e'<<8|'n')) then + sdoval=REOPEN 2 0 + if (sdoval) then + PRINT "Error Reopen" + endif + elseif (idx == 0x3003 and sdoval == ('e'<<24|'x'<<16|'i'<<8|'t')) then + CANOUT sdotx (canhi&0xffffff | 0x60000000) 0 + EXIT + elseif (idx == 0x3006) then + if (subidx == 0) then + if (sdoval == ('s'<<24|'t'<<16|'r'<<8|'t')) then + CVEL 0 + CSTART + elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then + CSTOP + endif + elseif (subidx == 1) then + CVEL sdoval + endif + elseif (idx == 0x3007) then + if (subidx==0 and sdoval == ('s'<<24|'y'<<16|'n'<<8|'c')) then + SYNCV + elseif (subidx==1 and sdoval == ('s'<<24|'y'<<16|'n'<<8|'c')) then + SYNCP + endif + elseif (idx == 0x3008) then + if (sdoval == ('o'<<24|'n'<<16)) then + NOWAIT ON + elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then + NOWAIT OFF + endif + elseif (idx == 0x4000) then + timeouttime = TIME + guardtime*lifetimefactor + elseif (idx == 0x6000) then + if (sdoval&1) then + SET POSDRCT -1 + else + SET POSDRCT 1 + endif + elseif (idx == 0x6002) then + SET VELRES sdoval + elseif (idx == 0x6003) then + if (subidx == 0 and sdoval == ('s'<<24|'e'<<16|'t'<<8)) then + DEF ORIGIN + elseif (subidx == 1) then + if (sdoval==0) then + RST ORIGIN + else + SET ORIGIN sdoval + endif + elseif (subidx == 2) then + SET HOME_OFFSET sdoval + endif + elseif (idx == 0x6004) then + if (subidx==0) then + POSA sdoval + elseif (subidx==1) then + POSR sdoval + endif + elseif (idx == 0x6200) then + pdotime = sdoval + if (pdo1on) then + ON PERIOD 0 GOSUB PROC_PDO1 + ON PERIOD pdotime GOSUB PROC_PDO1 + endif + elseif (idx == 0x6501) then + SET ENCODER sdoval + elseif (idx == 0x6502) then + SET VELMAX sdoval + else + CANOUT sdotx (canhi&0xffffff | (1<<31)/*&0x80000000*/) 0 + PRINT "Unknown SDO: idx=", idx, ", subidx=", subidx + goto ENDSDOSET + endif + + if (pdo1on) then + GOSUB PROC_PDO1 + endif + + CANOUT sdotx (canhi&0xffffff | 0x60000000) 0 +/* + PRINT "Sdo Set ", idx, "/", subidx +*/ + ENDSDOSET: + RETURN + + /*----------------------------------*/ + + SUBPROG PROC_SDOREQ + idx = canhi&0xff00 | (canhi>>16)&0xff + subidx = canhi&0xff +/* + PRINT "Requesting Idx:", idx, "/", subidx +*/ + if (idx == 0x1003) then + if (subidx >=0 and subidx<=9) then + sdoval = errlist[subidx-1] + endif + elseif (idx == 0x1004) then + if (subidx == 0) then + sdoval = 1 + elseif (subidx == 1) then + sdoval = 0 + elseif (subidx == 2) then + sdoval = 1 + endif + elseif (idx == 0x1005) then + sdoval = 1<<31 | 0x80 + elseif (idx == 0x100b) then + sdoval = nodenr + elseif (idx == 0x100a) then + sdoval = (kVERSION<<16) | kSUBVERSION + elseif (idx == 0x100c) then + sdoval = guardtime + elseif (idx == 0x100d) then + sdoval = lifetimefactor + elseif (idx == 0x100e) then + sdoval = 0x600|nodenr /*0x700 | nodenr*/ + elseif (idx == 0x1010) then + sdoval = 1 + elseif (idx == 0x1011) then + sdoval = 0 + elseif (idx == 0x1014) then + sdoval = 0x80 | nodenr + elseif (idx == 0x1800) then + if (subidx == 1) then + sdoval = (~pdo1on)<<31 | (0x0180 + nodenr) + elseif (subidx == 2) then + sdoval = 0xfe + elseif (subidx == 3) then + sdoval = 0 + endif + elseif (idx == 0x2000) then + if (subidx == 0) then + sdoval = GET POSERR + elseif (subidx == 1) then + sdoval = GET NEGLIMIT | (GET SWNEGLIMACT) << 30 + elseif (subidx == 2) then + sdoval = GET POSLIMIT | (GET SWPOSLIMACT) << 31 + elseif (subidx == 3) then + sdoval = (GET SWNEGLIMACT) | ((GET SWPOSLIMACT)<<1) + endif + elseif (idx == 0x2001) then + sdoval = AXEND + elseif (idx == 0x2002) then + sdoval = AVEL + elseif (idx == 0x2003) then + if (subidx==0) then + sdoval = INB 0 + elseif (subidx>0 and subidx<2) then + sdoval = IN subidx + endif + elseif (idx == 0x2004) then + sdoval = STAT + elseif (idx == 0x6000) then + if (GET POSDRCT == 1) then + sdoval = 0 + elseif (GET POSDRCT == -1) then + sdoval = 1 + endif + elseif (idx == 0x6002) then + sdoval = GET VELRES + elseif (idx == 0x6003) then + sdoval = GET HOME_OFFSET + elseif (idx == 0x6004) then + if (subidx == 0) then + sdoval = APOS + elseif (subidx==1) then + sdoval = CPOS + endif + elseif (idx == 0x6501) then + sdoval = GET ENCODER + elseif (idx == 0x6502) then + sdoval = GET VELMAX + elseif (idx == 0x6508) thenä + sdoval = TIME + else + CANOUT sdotx (canhi&0xffffff | 0x80000000) 0 + goto ENDSDOREQ + endif + + canlo = (sdoval&0xff)<<24 | (sdoval&0xff00)<<8 | (sdoval>>8)&0xff00 | (sdoval>>24)&0xff + CANOUT sdotx (canhi&0xffffff | 0x43000000) canlo +/* + PRINT "Returning: ", sdoval +*/ + ENDSDOREQ: + RETURN + + /*----------------------------------------------------------------------*/ + /* PROC_SDORX */ + /*----------------------------------------------------------------------*/ + /* This procedure handles incoming objects, it is called from the main */ + /* loop. If the object ID (COB ID) identifies a SDO object, it is */ + /* whether it is a object to set data (write into the object */ + /* dictionary) or data is requested (read from object dictionary) */ + /* If it isn't a valid SDO a error message is send. */ + /* Remark: Only objects with the right node number are received by the */ + /* main loop. */ + /*----------------------------------------------------------------------*/ + + SUBPROG PROC_SDORX +/* --Echo-- + CANOUT sdotx canhi canlo +*/ + cmd = canhi>>24 + if (cmd==0x23 OR cmd==0x2B OR cmd==0x2F) then + gosub PROC_SDOSET + elseif (cmd == 0x40) then + gosub PROC_SDOREQ + else + PRINT "Unknown SDO cmd", cmd + CANOUT sdotx (canhi&0xffffff | 0x80000000) 0 + endif + ENDSDORX: + RETURN + + /*----------------------------------*/ + /* PROC_CANMSG */ + /* called if a canmsg with */ + /* cobid=2*CANNR+1 is received */ + /* Warning: This doesn't fit to */ + /* CanOpen specification */ + /*----------------------------------*/ + SUBPROG PROC_CANMSG + varnr = InMsg(-1) + PRINT "varnr=", varnr, "msgval=", msgval + RETURN + +/*-------------------------------------------------------------------------*/ +/* PDO 1 Interrupt */ +/*-------------------------------------------------------------------------*/ + SUBPROG PROC_PDO1 + CANOUT pdo1 AXEND APOS + RETURN + +/*-------------------------------------------------------------------------*/ +/* Timeout Interrupt */ +/*-------------------------------------------------------------------------*/ + SUBPROG PROC_Timeout + if (TIME > timeouttime) then + MOTOR STOP + + if (firsttimeout==0) then + + /* Tell the bus that an error occured */ + CANOUT pdo2 0 0 + + i = errlist[1] + 1 /* Fill status of array */ + while (i>2) do /* shift errors by one */ + errlist[i] = errlist[i-1] + i = i - 1 + endwhile /* set new errornumber */ + errlist[2] = 100 /* User Error #100 */ + if (errlist[1]<8) then /* write new size if enhanced */ + errlist[1] = errlist[1] + 1 + endif + + errinf = 0 + + PRINT "User Timeout!" + + /* tell the bus what exactly happened */ + CANOUT pdo2 errlist[2] errinf + endif + + firsttimeout = 1 + else + firsttimeout = 0 + endif + RETURN + +/*-------------------------------------------------------------------------*/ +/* Error sub proc */ +/*-------------------------------------------------------------------------*/ + SUBPROG PROC_ERROR + MOTOR STOP + + /* Tell the bus that an error occured */ + CANOUT pdo2 0 0 + + i = errlist[1] + 1 /* Fill status of array */ + while (i>2) do /* shift errors by one */ + errlist[i] = errlist[i-1] + i = i - 1 + endwhile /* set new errornumber */ + errlist[2] = ERRNO + if (errlist[1]<8) then /* write new size if enhanced */ + errlist[1] = errlist[1] + 1 + endif + + errinf = 0 + + /* check if the error is repairable and repair */ + if (errlist[2]==6) then + PRINT "No home forced!" + ERRCLR + elseif (errlist[2]==8) then + PRINT "Control deviation overflow." + ERRCLR + errinf = 0xaffe + elseif (errlist[2]==9) then + PRINT "Did'n find zero index." + ERRCLR + elseif (errlist[2]==11) then + poslsw = GET POSLIMIT + neglsw = GET NEGLIMIT + if ((GET SWNEGLIMACT) and APOS<=neglsw) then + PRINT "Negative software endswitch (", neglsw, ") activated at position ", APOS + SET SWNEGLIMACT 0 + ERRCLR + CVEL (vres%100) + ACC (10*vres%100) + DEC (10*vres%100) + POSA neglsw + 100 + SET SWNEGLIMACT 1 + errinf = -1 + elseif ((GET SWPOSLIMACT) and APOS>=poslsw) then + PRINT "Positive software endswitch (", poslsw, ") activated at position ", APOS + SET SWPOSLIMACT 0 + ERRCLR + CVEL (vres%100) /* 1% */ + ACC (10*vres%100) /* 10% */ + DEC (10*vres%100) /* 10% */ + POSA poslsw - 100 + SET SWPOSLIMACT 1 + errinf = 1 + else + PRINT "Software endswitch activated - command skipped. Pos: ", APOS + ERRCLR + endif + elseif (errlist[2]==25) then + /* FIXME: To handle this correct you must make sure, + that the endswitch numbers are negative */ + poslsw = -(GET I_POSLIMITSW) + neglsw = -(GET I_NEGLIMITSW) + if (IN poslsw == 0) then + PRINT "Positive endswitch activated at position ", APOS + SET I_POSLIMITSW 0 + ERRCLR + CVEL (vres%100) /* 1% */ + ACC (10*vres%100) /* 10% */ + DEC (10*vres%100) /* 10% */ + CSTART + WHILE (IN poslsw == 0) DO ENDWHILE + CSTOP + SET I_POSLIMITSW -poslsw + errinf = 1 + elseif (IN neglsw == 0) then + PRINT "Negative endswitch activated at position ", APOS + SET I_NEGLIMITSW 0 + ERRCLR + vres = GET VELRES + CVEL -(vres%100) /* 1% */ + ACC (10*vres%100) /* 10% */ + DEC (10*vres%100) /* 10% */ + CSTART + WHILE (IN neglsw == 0) DO ENDWHILE + CSTOP + SET I_NEGLIMITSW -neglsw + errinf = -1 + endif + elseif (errlist[2]==84) then + PRINT "Too many (>12) ON TIME interrupts." + ERRCLR + ELSE + PRINT "Error Function Called: ERRNO=", errlist[2] + endif + + /* tell the bus what exactly happened */ + CANOUT pdo2 errlist[2] errinf + RETURN + +/*-------------------------------------------------------------------------*/ +/* End of part for Programs called with GOSUB */ +/*-------------------------------------------------------------------------*/ + +ENDPROG