Index: trunk/MagicSoft/Cosy/aposs/Magic.m =================================================================== --- trunk/MagicSoft/Cosy/aposs/Magic.m (revision 808) +++ trunk/MagicSoft/Cosy/aposs/Magic.m (revision 925) @@ -10,7 +10,12 @@ /* */ kVERSION = 0 /* */ -kSUBVERSION = 62 /* */ +kSUBVERSION = 63 /* */ /* */ /* HISTORY: */ +/* */ +/* * V0.63: */ +/* - added movement handshake timeout (removed 0x400 WAITAX) */ +/* - added brackets around string SDOs */ +/* - changed SDO 0x3007 to support both sync modes /* */ /* * V0.62: */ @@ -92,6 +97,10 @@ /* 0x3006 0 wo Velocity Mode 'strt', 'stop' */ /* 1 wo VelMode Velocity */ -/* 0x3007 x wo Syncronisation 'sync' */ +/* 0x3007 0 wo Velocity Syncronisation 'sync' */ +/* 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 */ /* 0x6000 x rw Rotation Direction */ /* 0x6002 x rw Velocity Resolution */ @@ -185,6 +194,16 @@ pdo1on = kFALSE -/*-------------------------------------------------------------------------*/ -/* Can Open Difinitions */ +timeout = 100 +timeouton = kFALSE +timeouttime = TIME + +/*-------------------------------------------------------------------------*/ +/* 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 @@ -195,9 +214,15 @@ sdotx = DEFCANOUT (0x580+nodenr) 8 sdorx = DEFCANIN (0x600+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 @@ -209,4 +234,9 @@ /*-------------------------------------------------------------------------*/ /* 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 */ @@ -221,4 +251,9 @@ /*-------------------------------------------------------------------------*/ /* 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: @@ -261,5 +296,5 @@ i = i - 1 endwhile - elseif (idx == 0x1010 and sdoval == 's'<<24|'a'<<16|'v'<<8|'e') then + elseif (idx == 0x1010 and sdoval == ('s'<<24|'a'<<16|'v'<<8|'e')) then SAVEPROM elseif (idx == 0x1800 and subidx == 1) then @@ -293,13 +328,13 @@ endif elseif (idx == 0x3000) then - if (sdoval == 'o'<<24|'n'<<16) then + if (sdoval == ('o'<<24|'n'<<16)) then MOTOR ON - elseif (sdoval == 'o'<<24|'f'<<16|'f'<<8) then + elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then MOTOR OFF - elseif (sdoval == 's'<<24|'t'<<16|'o'<<8|'p') then + 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 + if (sdoval == ('h'<<24|'o'<<16|'m'<<8|'e')) then limitsw = GET I_POSLIMITSW set I_POSLIMITSW 0 @@ -313,18 +348,18 @@ SET I_POSLIMITSW limitsw endif - elseif (idx == 0x3002 and sdoval == 'o'<<24|'p'<<16|'e'<<8|'n') then + 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 + 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 (sdoval == ('s'<<24|'t'<<16|'r'<<8|'t')) then CVEL 0 CSTART - elseif (sdoval == 's'<<24|'t'<<16|'o'<<8|'p') then + elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then CSTOP endif @@ -332,11 +367,35 @@ CVEL sdoval endif - elseif (idx == 0x3007 and sdoval == 's'<<24|'y'<<16|'n'<<8|'c') then - SYNCP + 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 + if (sdoval == ('o'<<24|'n'<<16)) then NOWAIT ON - elseif (sdoval == 'o'<<24|'f'<<16|'f'<<8) then + elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then NOWAIT OFF + 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 @@ -356,5 +415,5 @@ SET VELRES sdoval elseif (idx == 0x6003) then - if (subidx == 0 and sdoval == 's'<<24|'e'<<16|'t'<<8) then + if (subidx == 0 and sdoval == ('s'<<24|'e'<<16|'t'<<8)) then DEF ORIGIN elseif (subidx == 1) then @@ -384,5 +443,6 @@ SET VELMAX sdoval else - CANOUT sdotx (canhi&0xffffff | 0x80000000) 0 + CANOUT sdotx (canhi&0xffffff | (1<<31)/*&0x80000000*/) 0 + PRINT "Unknown SDO: idx=", idx, ", subidx=", subidx goto ENDSDOSET endif @@ -464,5 +524,9 @@ sdoval = STAT elseif (idx == 0x4000) then - WAITAX + if (subidx == 1) then + sdoval = timeouton + elseif (subidx == 2) then + sdoval = timeout + endif elseif (idx == 0x6000) then if (GET POSDRCT == 1) then @@ -500,5 +564,15 @@ 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 @@ -536,4 +610,37 @@ CANOUT pdo1 AXEND APOS RETURN + +/*-------------------------------------------------------------------------*/ +/* Timeout Interrupt */ +/*-------------------------------------------------------------------------*/ + SUBPROG PROC_Timeout + if (TIME > timeouttime) then + 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] = 100 /* User Error #100 */ + if (errlist[1]<8) then /* write new size if enhanced */ + errlist[1] = errlist[1] + 1 + endif + + errinf = 0 + + /*if (firstcall)*/ + PRINT "User Timeout at uptime=", (TIME%1000), "s" + /*firstcall = kFALSE */ + + /* tell the bus what exactly happened */ + CANOUT pdo2 errlist[2] errinf + /*elseif + firstcall = kTRUE*/ + endif + RETURN /*-------------------------------------------------------------------------*/