/*-------------------------------------------------------------------------*/
/* DIM section for Array definition (arrays are globals)                   */
/*-------------------------------------------------------------------------*/

DIM errlist[9] /* idx 1=number of valid entries */           

/* ----------------------------------------------------------------------- */
/*                                                                         */               
/*  Version:                                                               */
/*                                                                         */
kVERSION    = 0   /*                                                       */
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:                                                              */
/*       - 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                                               */
/*   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                                       */
/*          1 rw Enable timeout timer, 'on' (1), 'off'(0)                  */
/*          2 rw Timeout timer time                                        */
/*   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    1          /* 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

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
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
                               
/* 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 == 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
         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
         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 == 0x100e) then
         sdoval = 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 == 0x4000) then
         if (subidx == 1) then
            sdoval = timeouton
         elseif (subidx == 2) then
            sdoval = timeout               
         endif
      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                          
      diff = timeouttime - TIME
      if (diff < 0) 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 missed=", -diff, "ms"
         /*firstcall = kFALSE  */

         /* tell the bus what exactly happened */
         CANOUT pdo2 errlist[2] errinf
      /*elseif
         firstcall = kTRUE*/
      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 poslsw == 0) DO ENDWHILE
            CSTOP
            SET I_NEGLIMITSW -poslsw
            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
