/* ----------------------------------------------------------------------- */
/*                                                                         */               
/*  Version:                                                               */
/*                                                                         */
kVERSION    = 0   /*                                                       */
kSUBVERSION = 1   /*                                                       */
/*                                                                         */
/*  HISTORY:                                                               */
/*                                                                         */
/*   * V0.1:                                                               */
/*       - first implementation                                            */
/*                                                                         */
/* ----------------------------------------------------------------------- */
                          
PRINT "Magic Manc (Manual Control) V", kVERSION, ".", kSUBVERSION /*       */

/*
if (get cannr!=1) and (get cannr!=3) then
   PRINT "Sorry, wrong MACS (CAN Id=", get cannr, ") only #1 and #3 allowed!"
   exit
endif   
*/

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

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

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

/*-------------------------------------------------------------------------*/
/* Make sure 'Reglerfreigabe' not set                                      */
/*-------------------------------------------------------------------------*/
RF = 0
out 1 0
out 2 0     

/*-------------------------------------------------------------------------*/
/* Error routine                                                           */
/*-------------------------------------------------------------------------*/
on error gosub suberror
                   
kIoModule = 4*256                    

/*-------------------------------------------------------------------------*/
/* mainloop rotation mode but stand still                                  */
/*-------------------------------------------------------------------------*/
if (get cannr==1) or (get cannr==3) then
   cvel 0
   cstart
   print "Starting..."             
else          
   print "Synchronizing..."             
   syncv
endif
          
mainloop:       
   fuse  = in (kIoModule+1)
   emcy  = in (kIoModule+2)
   vltg  = in (kIoModule+3)
   mode  = in (kIoModule+4)
   if (get cannr==1) or (get cannr==2) then
       ready = in 1
   elseif get cannr==3 then
      ready = in (kIoModule+5)
   endif   
       
   if fuse==0 then 
      print "Motor-Power Fuse not OK!"
      waitt 500
      goto mainloop
   elseif emcy==0 then 
      print "Please release Emergency Stop!"
      waitt 500 
      goto mainloop
   elseif vltg==0 then 
      print "Overvoltage control broken!"
      waitt 500 
      goto mainloop
/*      
   elseif mode==1 then
      print "Control not in manual mode!"
      out 1 0
      out 2 0
      waitt 500
      exit
*/                 
   elseif (ready==0) and (RF==1) then
      print "DKC not ready... setting RF=AH=0!"
      out 1 0
      out 2 0
      cvel 0   
      RF = 0           
      waitt 1000       
      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
      waitt 100
      def origin
      out 1 1
      out 2 1                                 
      RF = 1
      waitt 100
   elseif (ready==0) or (RF==0) then
      goto mainloop
   endif         
/*       
   if (get cannr==2) then
      print "M:", mapos, " ", mavel, "  S: ", apos, " ", avel, "  C:", cpos 
      waitt 500      
      goto mainloop
   endif       
*/       
   forward  = in 2
   backward = in 3       
                            
   if     (forward==1) and (backward==0) then
      cvel  manvel             
   elseif (forward==0) and (backward==1) then
      cvel -manvel
   else
      cvel 0       
   endif              
goto mainloop       
       
SUBMAINPROG 
   subprog suberror
      out 1 0
      out 2 0
      RF = 0
      waitt 100      

      print "Error #", errno
      
      if errno==3 then     /* axis not existing: shoud never happen */
         exit
      elseif errno==5 then /* error remaining: tried moving while error not cleared */ 
         /* !!! */
         exit
      elseif errno==6 then /* home not first command: shoud never happen */
         exit
      elseif errno==8 then /* control deviation too large */
         /*
          *
          */
      elseif errno==9 then /* index not found: shoud never happen */
         exit                
      elseif errno==10 then /* unknown command: shoud never happen */   
         exit
      elseif errno==11 then /* software endswitch reached */
         /*
          *
          */
      elseif errno==12 then /* wrong paremeter number: shoud never happen */
         exit
      elseif errno==14 then /* too many LOOP commands: shoud never happen */
         exit
      elseif errno==16 then /* parameter in EEPROM broken */
         exit
      elseif errno==17 then /* programs in EEPROM broken */
         exit
      elseif errno==18 then /* RESET by CPU: Reason could be power-problems */
         exit
      elseif errno==19 then /* User break */
         exit
      elseif errno==25 then /* hardware Endswitch reached */
         /*
          *
          */
      elseif errno==51 then /* too many gosub: shoud never happen */
         exit
      elseif errno==52 then /* too many return: shoud never happen */
         exit
      elseif errno==62 then /* error verifying EEPROM */
         exit
      elseif errno==70 then /* error in DIM statement: should never happen */
         exit
      elseif errno==72 then /* DIM limit reached: should never happen */
         exit
      elseif errno==79 then /* Timeout waiting for an index */
         exit
      elseif errno==84 then /* Too many ON TIME calls */
         exit                                        
      elseif errno==87 then /* storage for variables exhausted */
         exit    
      else
         print "Unknown (internal) error #", errno
         exit
      endif
                 
      exit           
      /*errclr*/ /* errclr includes 'motor on' which enables the motor controlling */
      
   return
ENDPROG


