/*-------------------------------------------------------------------------*/ /* DIM section for Array definition (arrays are globals) */ /*-------------------------------------------------------------------------*/ DIM errlist[9] /* idx 1=number of valid entries */ /* ----------------------------------------------------------------------- */ /* */ /* Version: */ /* */ kVERSION = 0 /* */ kSUBVERSION = 70 /* */ /* */ /* HISTORY: */ /* */ /* * V0.70: */ /* - Implemented switch off of motor in case of Input 6 has a */ /* falling edge */ /* - moved the arbitrary factor 4 for the tracking speed from */ /* cosy to here */ /* */ /* * V0.69: */ /* - Implemented Pre-Operational/Operational state */ /* means: + cannot start RPM mode */ /* + SDO 0x6004 (POSA/POSR) ignored */ /* + new SDO 0x1000 */ /* */ /* * V0.68: */ /* - Made NoWait=On the default */ /* - Implemented error 89 (CAN I/O) */ /* - changed "REOPEN 0 0" to "REOPEN 1 0" */ /* */ /* * 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 */ /* */ /* * 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, " (CANbus controlled)" /**/ /* ----------------------------------------------------------------------- */ /* */ /* Object Dictionary: */ /* */ /* 0x1000 1 rw Start node (Pre-oprational --> operational) */ /* 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' (Warning: Motor off can have */ /* strange side effects if RF=1!) */ /* 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 -1 /* Don't restart any Program on Exit */ SET ENCODERTYPE 0 /* Incremental Encoder */ SET MENCODERTYPE 0 /* Incremental Encoder (Master) */ 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 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 */ /*----------------*/ /* syncronisation */ /*----------------*/ 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 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 */ 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 */ /*----------------*/ /* 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 */ /*----------------*/ /* OLD ORIGINAL VALUES: Prop=100, Div=300, Int=1000 */ if (get cannr==1) then SET KPROP 100 /*950*/ SET KDER 300 /*2000*/ SET KINT 1000 /*0*/ 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 */ /* OLD: SET HOME_VEL -(25*vres%100) /* Home position velocity: 25% */ /* OLD: SET HOME_RAMP (25*vres%100) /* Home position accel: 25% */ /*----------------*/ /* Manual control */ /*----------------*/ 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"*/ /*-------------------------------------------------------------------------*/ /* const section for constant velues */ /*-------------------------------------------------------------------------*/ kTRUE = 1 kFALSE = 0 pdotime = 100 pdo1on = kFALSE guardtime = 0 lifetimefactor = 0 timeouttime = TIME firsttimeout = 0 preop = kTRUE /*-------------------------------------------------------------------------*/ /* 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 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 1 0 /*-------------------------------------------------------------------------*/ /* 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 OUT 1 1 */ NOWAIT ON /*-------------------------------------------------------------------------*/ /* 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 /***************************************************************************/ kIoModule = 4*256 /*-------------------------------------------------------------------------*/ /* Init */ /*-------------------------------------------------------------------------*/ /* 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 brake = 0 RF = 0 init = 0 oldstate = kTRUE gosub reset PRINT "Starting Mainloop..." rc = 1 MAINLOOP: 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) door = in (kIoModule+6) ready = in 1 if (door==0) then preop = kTRUE endif 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 /* This "motor on" is necessary to make sure the floating analog output doesn't move the telescope (slowly) */ 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 if (preop==kTRUE and oldstate!=preop) then print "Door switch changed state!" gosub reset endif oldstate = preop rc = CANIN sdorx -1 0 canhi canlo goto mainloop ENDMAIN: MOTOR STOP MOTOR OFF OUT 1 0 EXIT /*-------------------------------------------------------------------------*/ /* Part for Programs called with GOSUB */ /*-------------------------------------------------------------------------*/ 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 */ /*----------------------------------*/ 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 canlo = 0 /* PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval */ if (idx==0x1000 and subidx==1) then preop = kTRUE if (sdoval==1 and door==1) then preop = kFALSE else canlo = 1 endif elseif (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 /* If velocity mode active */ /*if (AXEND&4) DEC sdoval%4 else*/ DEC sdoval /*endif*/ else /*if (AXEND&4) ACC sdoval%4 else*/ ACC sdoval /*endif*/ 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 if (preop==kFALSE) then CVEL 0 CSTART else canlo = 1 endif elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then CSTOP endif elseif (subidx == 1) then /* It is unclear where this arbitraty factor comes from */ CVEL sdoval%4 endif elseif (idx == 0x3007) then if (preop==kFALSE) 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 else canlo = 1 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 (preop==kFALSE) then if (subidx==0) then POSA sdoval elseif (subidx==1) then POSR sdoval endif else canlo = 1 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) canlo /* 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 == 0x1000 and subidx==1) then sdoval = preop elseif (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*4 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|(((~preop)&1)<<8)) APOS RETURN /*-------------------------------------------------------------------------*/ /* FENCE OPENED */ /*-------------------------------------------------------------------------*/ SUBPROG DOOR_STATUS_CHANGED preop = kTRUE 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*/ 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 */ 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 elseif (errlist[2]==89) then PRINT "CAN I/O error." errinf = REOPEN 1 0 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