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

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

/* ----------------------------------------------------------------------- */
/*                                                                         */               
/*  Version:                                                               */
/*                                                                         */
kVERSION    = 0   /*                                                       */
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                   */
/*                                                                         */
/*   * 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:                                                     */
/*                                                                         */
/*   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       -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 */
/*----------------*/ 

/* 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          */


/* 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

/*-------------------------------------------------------------------------*/
/* 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 0 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 OFF

/*-------------------------------------------------------------------------*/
/* 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

   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)   
         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

      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
/*
      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*/
      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
      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
