/* ----------------------------------------------------------------------- */
/*                                                                         */               
/*  Version:                                                               */
/*                                                                         */
kVERSION    = 0   /*                                                       */
kSUBVERSION = 6   /*                                                       */
/*                                                                         */
/*  HISTORY:                                                               */
/*                                                                         */
/*   * V0.6                                                                */
/*       - do not reset the velocity if set already                        */
/*       - set acceleration to 20%                                         */
/*                                                                         */
/*   * V0.5:                                                               */
/*       - changes 'DKC Ready' signal to IN1 for all MACS                  */
/*       - control brake only when cannr==3                                */
/*                                                                         */
/*   * V0.4:                                                               */
/*       - restart MACS with PRGPAR 1 when not in manual mode              */
/*       - replaced label reset by a subprg                                */
/*                                                                         */
/*   * V0.3:                                                               */
/*       - added support for the elevation axis brake                      */
/*       - enabled check for operation mode (remote control/pc)            */
/*       - added 'reset' label                                             */
/*       - moved syncv/cstart to setting rf                                */
/*                                                                         */
/*   * V0.2:                                                               */
/*       - fixed a bug, shaking the telescope switching on again after an  */
/*         emergency stop                                                  */
/*                                                                         */
/*   * 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 = 10*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,  Acc=", defacc

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

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

/*-------------------------------------------------------------------------*/
/* mainloop rotation mode but stand still                                  */
/*-------------------------------------------------------------------------*/
                              
brake = 0
RF    = 0
velo  = 0
   
gosub reset
         
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 (ready==0) and (RF==1) then
      print "DKC not ready, but RF set... setting RF=AH=0!"
      gosub reset
      goto mainloop
   elseif mode==0 then
      print "Control not in manual mode!"
      gosub reset  
      SET PRGPAR 1
      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
         
      if (get cannr==2) then
         syncv
         print "Synchronizing speed..."             
      else   
         cstart
         print "Starting revolution mode..."             
         waitt 500
      endif   
   elseif (ready==0) or (RF==0) then
      goto mainloop
   endif         
/*       
   if (get cannr==2) then
      print apos, " ", mapos, "  ", avel," ", mavel
      waitt 500
      goto mainloop
   endif       
*/       
   forward  = in 2
   backward = in 3       
                            
   if     (forward==1) and (backward==0) and (velo!=manvel) then
      cvel  manvel                                          
      velo = manvel
   elseif (forward==0) and (backward==1) and (velo!=-manvel) then
      cvel -manvel
      velo = -manvel
   elseif (forward==backward) and (velo!=0) then
      cvel 0
      velo = 0       
   endif              
goto mainloop       
       
SUBMAINPROG 
   subprog reset
      out 1 0
      out 2 0   
      RF = 0
      motor off    
      velo = 0
      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   
   return
      
   subprog suberror
      out 1 0
      out 2 0
      RF = 0
      waitt 100
      velo = 0
      
      if (brake==1 and get cannr==3) then
         waitt 5000
         out (kIoModule+1) 0
         waitt 500
      endif         

      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


