Ignore:
Timestamp:
09/07/01 12:44:16 (23 years ago)
Author:
tbretz
Message:
*** empty log message ***
File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/MagicSoft/Cosy/aposs/Magic.m

    r808 r925  
    1010/*                                                                         */
    1111kVERSION    = 0   /*                                                       */
    12 kSUBVERSION = 62  /*                                                       */
     12kSUBVERSION = 63  /*                                                       */
    1313/*                                                                         */
    1414/*  HISTORY:                                                               */
     15/*                                                                         */
     16/*   * V0.63:                                                              */
     17/*       - added movement handshake timeout (removed 0x400 WAITAX)         */
     18/*       - added brackets around string SDOs                               */
     19/*       - changed SDO 0x3007 to support both sync modes
    1520/*                                                                         */
    1621/*   * V0.62:                                                              */
    … …  
    9297/*   0x3006 0 wo Velocity Mode 'strt', 'stop'                              */
    9398/*          1 wo VelMode Velocity                                          */
    94 /*   0x3007 x wo Syncronisation 'sync'                                     */
     99/*   0x3007 0 wo Velocity Syncronisation 'sync'                            */
     100/*          1 wo Position Syncronisation 'sync'                            */
    95101/*   0x3008 x wo Nowait 'on', 'off'                                        */
     102/*   0x4000 0 wo Reset timeout timer                                       */
     103/*          1 rw Enable timeout timer, 'on' (1), 'off'(0)                  */
     104/*          2 rw Timeout timer time                                        */
    96105/*   0x6000 x rw Rotation Direction                                        */
    97106/*   0x6002 x rw Velocity Resolution                                       */
    … …  
    185194pdo1on  = kFALSE
    186195
    187 /*-------------------------------------------------------------------------*/
    188 /* Can Open Difinitions                                                    */
     196timeout     = 100
     197timeouton   = kFALSE   
     198timeouttime = TIME
     199
     200/*-------------------------------------------------------------------------*/
     201/* Can Open Definitions                                                    */
     202/*-------------------------------------------------------------------------*/
     203/* The CAN Object are static object. This is why they must be deleted.     */
     204/* The program should run in any of our nodes.                             */
     205/* Therefor the standard CAN objects (SDO, PDO1, PDO2) for communication   */
     206/* are defined. The nodenumber is part of the object ID (this is somehow   */
     207/* similar to the TCP/IP ports)                                            */
    189208/*-------------------------------------------------------------------------*/
    190209CANDEL -1
    … …  
    195214sdotx = DEFCANOUT (0x580+nodenr) 8
    196215sdorx = DEFCANIN  (0x600+nodenr) 8
    197 
     216                               
     217/* Close and reopen communication, enable buffering */                               
    198218err = REOPEN 0 0
    199219
    200220/*-------------------------------------------------------------------------*/
    201221/* Init                                                                    */
     222/*-------------------------------------------------------------------------*/
     223/* Before the motor control hardware is enabled (hi on output 1)           */
     224/* the commands make sure, that the motor will not start moving.           */
     225/* As default positioning commands doesn't stop the further execution      */
     226/* of the program.                                                         */
    202227/*-------------------------------------------------------------------------*/
    203228MOTOR  STOP
    … …  
    209234/*-------------------------------------------------------------------------*/
    210235/* ON ... GOSUB ... definitions                                            */
     236/*-------------------------------------------------------------------------*/
     237/* The errorlist one can retreive using the corresponding CAN object       */
     238/* should be emty when the node is initialized (arrays are static objects) */
     239/* therefor it must be deleted.                                            */
     240/* Errors are handled in an interrupt procedure called PROC_ERROR          */
    211241/*-------------------------------------------------------------------------*/
    212242/* ON CANMSG GOSUB PROC_CANMSG */
    … …  
    221251/*-------------------------------------------------------------------------*/
    222252/* Program Main Loop                                                       */
     253/*-------------------------------------------------------------------------*/
     254/* The main loop is the core of the program which handles incoming         */
     255/* objects. In principal CANIN should wait until an object is received,    */
     256/* but it stops waiting when an interrupt occurs. This is the reason why   */
     257/* the validity of the message must be checked.                            */
    223258/*-------------------------------------------------------------------------*/
    224259MAIN:
    … …  
    261296            i = i - 1
    262297         endwhile
    263       elseif (idx == 0x1010 and sdoval == 's'<<24|'a'<<16|'v'<<8|'e') then
     298      elseif (idx == 0x1010 and sdoval == ('s'<<24|'a'<<16|'v'<<8|'e')) then
    264299         SAVEPROM
    265300      elseif (idx == 0x1800 and subidx == 1) then
    … …  
    293328        endif
    294329      elseif (idx == 0x3000) then
    295           if (sdoval == 'o'<<24|'n'<<16) then
     330          if (sdoval == ('o'<<24|'n'<<16)) then
    296331              MOTOR ON
    297           elseif (sdoval == 'o'<<24|'f'<<16|'f'<<8) then
     332          elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then
    298333              MOTOR OFF                                     
    299           elseif (sdoval == 's'<<24|'t'<<16|'o'<<8|'p') then
     334          elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then
    300335              MOTOR STOP
    301336          endif
    302337      elseif (idx == 0x3001) then
    303           if (sdoval == 'h'<<24|'o'<<16|'m'<<8|'e') then
     338          if (sdoval == ('h'<<24|'o'<<16|'m'<<8|'e')) then
    304339            limitsw = GET I_POSLIMITSW     
    305340            set I_POSLIMITSW 0     
    … …  
    313348            SET I_POSLIMITSW limitsw
    314349         endif
    315       elseif (idx == 0x3002 and sdoval == 'o'<<24|'p'<<16|'e'<<8|'n') then
     350      elseif (idx == 0x3002 and sdoval == ('o'<<24|'p'<<16|'e'<<8|'n')) then
    316351         sdoval=REOPEN 2 0
    317352         if (sdoval) then
    318353             PRINT "Error Reopen"
    319354         endif
    320       elseif (idx == 0x3003 and sdoval == 'e'<<24|'x'<<16|'i'<<8|'t') then
     355      elseif (idx == 0x3003 and sdoval == ('e'<<24|'x'<<16|'i'<<8|'t')) then
    321356         CANOUT sdotx (canhi&0xffffff | 0x60000000) 0
    322357         EXIT
    323358      elseif (idx == 0x3006) then
    324359         if (subidx == 0) then
    325             if (sdoval == 's'<<24|'t'<<16|'r'<<8|'t') then
     360            if (sdoval == ('s'<<24|'t'<<16|'r'<<8|'t')) then
    326361               CVEL 0
    327362               CSTART
    328              elseif (sdoval == 's'<<24|'t'<<16|'o'<<8|'p') then
     363             elseif (sdoval == ('s'<<24|'t'<<16|'o'<<8|'p')) then
    329364               CSTOP
    330365             endif
    … …  
    332367            CVEL sdoval
    333368         endif
    334       elseif (idx == 0x3007 and sdoval == 's'<<24|'y'<<16|'n'<<8|'c') then
    335          SYNCP
     369      elseif (idx == 0x3007) then
     370         if (subidx==0 and sdoval == ('s'<<24|'y'<<16|'n'<<8|'c')) then
     371            SYNCV
     372         elseif (subidx==1 and sdoval == ('s'<<24|'y'<<16|'n'<<8|'c')) then
     373            SYNCP
     374         endif   
    336375      elseif (idx == 0x3008) then
    337          if (sdoval == 'o'<<24|'n'<<16) then
     376         if (sdoval == ('o'<<24|'n'<<16)) then
    338377             NOWAIT ON
    339          elseif (sdoval == 'o'<<24|'f'<<16|'f'<<8) then
     378         elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then
    340379             NOWAIT OFF
     380         endif
     381      elseif (idx == 0x4000) then
     382         if (subidx == 0) then         
     383            timeouttime = TIME + timeout                 
     384         elseif (subidx == 1) then
     385            ON PERIOD 0 GOSUB PROC_Timeout
     386            if (sdoval == ('o'<<24|'n'<<16)) then
     387               timeouttime = TIME + timeout
     388               ON PERIOD timeout GOSUB PROC_Timeout
     389               timeouton = kTRUE                               
     390            elseif (sdoval == ('o'<<24|'f'<<16|'f'<<8)) then
     391               timeouton = kFALSE
     392            endif
     393         elseif (subidx == 2) then
     394            timeout = sdoval             
     395            if (timeouton) then
     396               ON PERIOD 0 GOSUB PROC_Timeout
     397               timeouttime = TIME + timeout
     398               ON PERIOD timeout GOSUB PROC_Timeout
     399            endif
    341400         endif
    342401/*    elseif (idx == 0x4000 and
    … …  
    356415         SET VELRES sdoval
    357416      elseif (idx == 0x6003) then
    358          if (subidx == 0 and sdoval == 's'<<24|'e'<<16|'t'<<8) then
     417         if (subidx == 0 and sdoval == ('s'<<24|'e'<<16|'t'<<8)) then
    359418            DEF ORIGIN   
    360419         elseif (subidx == 1) then
    … …  
    384443         SET VELMAX sdoval   
    385444      else
    386          CANOUT sdotx (canhi&0xffffff | 0x80000000) 0
     445         CANOUT sdotx (canhi&0xffffff | (1<<31)/*&0x80000000*/) 0
     446         PRINT "Unknown SDO: idx=", idx, ", subidx=", subidx
    387447         goto ENDSDOSET
    388448      endif
    … …  
    464524         sdoval = STAT
    465525      elseif (idx == 0x4000) then
    466          WAITAX
     526         if (subidx == 1) then
     527            sdoval = timeouton
     528         elseif (subidx == 2) then
     529            sdoval = timeout               
     530         endif
    467531      elseif (idx == 0x6000) then
    468532         if (GET POSDRCT == 1) then
    … …  
    500564   RETURN
    501565
    502    /*----------------------------------*/
     566   /*----------------------------------------------------------------------*/
     567   /*                              PROC_SDORX                              */
     568   /*----------------------------------------------------------------------*/
     569   /* This procedure handles incoming objects, it is called from the main  */
     570   /* loop. If the object ID (COB ID) identifies a SDO object, it is       */
     571   /* whether it is a object to set data (write into the object            */
     572   /* dictionary) or data is requested (read from object dictionary)       */
     573   /* If it isn't a valid SDO a error message is send.                     */
     574   /* Remark: Only objects with the right node number are received by the  */
     575   /*         main loop.                                                   */
     576   /*----------------------------------------------------------------------*/
    503577
    504578   SUBPROG PROC_SDORX
    … …  
    536610      CANOUT pdo1 AXEND APOS
    537611   RETURN
     612
     613/*-------------------------------------------------------------------------*/
     614/* Timeout Interrupt                                                       */
     615/*-------------------------------------------------------------------------*/
     616   SUBPROG PROC_Timeout
     617      if (TIME > timeouttime) then                                 
     618         MOTOR STOP
     619         
     620         /* Tell the bus that an error occured */
     621         CANOUT pdo2 0 0
     622     
     623         i = errlist[1] + 1              /* Fill status of array       */
     624         while (i>2) do                  /* shift errors by one        */
     625            errlist[i] = errlist[i-1]
     626            i = i - 1
     627         endwhile                        /* set new errornumber        */
     628         errlist[2] = 100                /* User Error #100            */
     629         if (errlist[1]<8) then          /* write new size if enhanced */
     630            errlist[1] = errlist[1] + 1
     631         endif     
     632
     633         errinf = 0
     634                         
     635         /*if (firstcall)*/
     636            PRINT "User Timeout at uptime=", (TIME%1000), "s"
     637         /*firstcall = kFALSE  */
     638
     639         /* tell the bus what exactly happened */
     640         CANOUT pdo2 errlist[2] errinf
     641      /*elseif
     642         firstcall = kTRUE*/
     643      endif   
     644   RETURN   
    538645
    539646/*-------------------------------------------------------------------------*/
Note: See TracChangeset for help on using the changeset viewer.