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

DIM errlist[9]           

/* ----------------------------------------------------------------------- */
/*                                                                         */               
/*  Version:                                                               */
/*                                                                         */
kVERSION    = 0   /*                                                       */
kSUBVERSION = 50  /*                                                       */
/*                                                                         */
/*  HISTORY:                                                               */
/*                                                                         */
/*   * 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                               */
/*          2 rw Positive 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                                          */
/*   0x3008 x wo Nowait 'on', 'off'                                        */
/*   0x6000 x rw Rotation Direction                                        */
/*   0x6002 x rw Velocity Resolution                                       */
/*   0x6003 0 wo Define present position as origin                         */
/*          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 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               */

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

/*-------------------------------------------------------------------------*/
/* Can Open Difinitions                                                    */
/*-------------------------------------------------------------------------*/
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

err = REOPEN 0 0

/*-------------------------------------------------------------------------*/
/* Init                                                                    */
/*-------------------------------------------------------------------------*/
MOTOR  STOP
MOTOR  OFF
CVEL   0
NOWAIT OFF
OUT    1 1

/*-------------------------------------------------------------------------*/
/* ON ... GOSUB ... definitions                                            */
/*-------------------------------------------------------------------------*/
/* ON CANMSG GOSUB PROC_CANMSG */
i = 8
while (i) do
   errlist[i] = 0
   i = i - 1
endwhile
 
ON ERROR GOSUB PROC_ERROR

/*-------------------------------------------------------------------------*/
/* Program Main Loop                                                       */
/*-------------------------------------------------------------------------*/
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 = 0
         while (i<9) 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  
              if ((sdoval>>30)&1) then
                 SET NEGLIMIT    sdoval & 0x3fffffff
                 SET SWNEGLIMACT 1
              else
                 SET SWNEGLIMACT 0
              endif 
           elseif (subidx == 2) then
              if ((sdoval>>31)&1) then
                 SET POSLIMIT    sdoval & 0x3fffffff
                 SET SWPOSLIMACT 1
              else
                 SET SWPOSLIMACT 0
              endif 
           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 == 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 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 == 0x6001) then
         SET ENCODER sdoval*/
      elseif (idx == 0x6002) then
         SET VELRES sdoval
      elseif (idx == 0x6003) then
         if (subidx == 0) 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 | 0x80000000) 0
         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]
         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
           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
         WAITAX
      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

   /*----------------------------------*/

   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

/*-------------------------------------------------------------------------*/
/* Error sub proc                                                          */
/*-------------------------------------------------------------------------*/
   SUBPROG PROC_ERROR
      MOTOR STOP
      errinf = 0

      /* Tell the bus that an error occured */
      CANOUT pdo2 0 0
      
      i = errlist[0] + 1        
      while (i>1) do
         errlist[i] = errlist[i-1]
         i = i - 1
      endwhile  
      errlist[1] = ERRNO
      if (errlist[0]<8) then
         errlist[0] = errlist[0] + 1
      endif

      /* check if the error is repairable and repair */
      if (errlist[1]==6) then
        PRINT "No home forced!"
        ERRCLR
      elseif (errlist[1]==8) then
        PRINT "Schleppabstand überschritten"
        ERRCLR
        errinf = 0xaffe
      elseif (errlist[1]==9) then
        PRINT "Did'n find zero index."
        ERRCLR
      elseif (errlist[1]==25) then
         lsw = -(GET I_POSLIMITSW)
         if (IN lsw == 0) then
            PRINT "Positive endswitch activated at position ", APOS
            SET I_POSLIMITSW 0
            ERRCLR
            CVEL  (vres%100) /* 1% */
            ACC   (10*vres%100)
            DEC   (10*vres%100)
            CSTART
            WHILE (IN lsw == 0) DO ENDWHILE
            CSTOP
            SET I_POSLIMITSW -lsw
            errinf = 1
         endif
         lsw = -(GET I_NEGLIMITSW)
         if (IN lsw == 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% */
            OUT 1 1
            MOTOR ON
            CSTART
            WHILE (IN lsw == 0) DO ENDWHILE
            CSTOP
            SET I_NEGLIMITSW -lsw
            errinf = -1
         endif
      elseif (errlist[1]==84) then
         PRINT "Too many (>12) ON TIME interrupts."
         ERRCLR
      ELSE
        PRINT "Error Function Called: ERRNO=", errlist[1]
      endif

	    /* tell the bus what exactly happened */
      CANOUT pdo2 errlist[1] errinf
   RETURN

/*-------------------------------------------------------------------------*/
/* End of part for Programs called with GOSUB                              */
/*-------------------------------------------------------------------------*/

ENDPROG