' ===============================================================================================
' (UK BPM MOTION CONTROL SYSTEM - ESA SLAC)
' COLLIMATOR DAMAGE TEST MOTION CONTROL SYSTEM - ATF KEK
' MINT CODE
' 
' Version: 5
' Author: Bino Maiheu, University College London (bino@hep.ucl.ac.uk)
'         Modified by Luis Fernandez-Hernando, STFC (j.l.fernandez.hernando@dl.ac.uk)
' Comments:
'   - 01.07.2007 : created, based upon template provided by Stewart Boogert for LaserWire project
'                  in KEK
'   - 02.07.2007 : added proper error handling for the limit switches and mechanical stops
'   - 03.07.2007 : added comms(10) which returns a status whether something is moving
'   - 05.07.2007 : added stopping of motion on selected axis/all axes
'   - 09.07.2007 : o set the pos on the moved axis to the encoder position after the
'                    move to compensate for long term discrepancies between encoder and pos
'                  o put in comms(21) as the code version running and abused comms(20)
'                    as 
'                  o added an abort handle, executes comms5 event !
'   - 27.11.2007 : trying to add a rotational axis
'   - 03.12.2007 : Adding new icommands for the rotational move
'
' Command setup of the COMMS array :
'  COMMS(1) - the command number to pass to the controller
'  COMMS(2) - the axis number
'  COMMS(3) - the distance if needed
'  COMMS(4) - the velocity if needed
'  COMMS(5) - trigger for a stop event on the selected axis, stops motion !
'  COMMS(6) - the number of the DIN requested
'  COMMS(7) - the status of the DIN in COMMS(6)
'  COMMS(8) - the encoder position for the selected axis
'
'  COMMS(10) - action indicator
'              0 : no action requested
'              1 : action in progress
'              2 : action done
'            - the user should take care of resetting the action to 0 after each call
'              this way he/she knows his/hers requested action was completed
'
'  COMMS(20) - the controller software status
'              1 : software online, and running
'  COMMS(21) - the iCodeVersion
'
'
' Command set to pass to the controller ( possible values of COMMS(1) )
' iCommand = 1 : enable current axis defined by COMMS(2)
' iCommand = 2 : disable current axis defined by COMMS(2)
' iCommand = 3 : get the encoder position of current axis into COMMS(8)
' iCommand = 4 : set the encoder position of current axis from COMMS(8)
' iCommand = 5 : execute a relative move
' iCommand = 6 : execute an absolute move
' iCommand = 7 : execute homing routine, routines 5,6,7 use COMMS(2) for axis, 
'                COMMS(3) for distance/position and COMMS(4) for speed
' iCommand = 8 : execute a relative rotation
' iCommand = 9 : execute an absolute rotation
' ===============================================================================================


' ===============================================================================================
' Variable Definitions
' ===============================================================================================
Dim iCodeVersion As Integer    ' the version of the code running on the controller

Dim nIteration As Integer      ' number of iterations in move loop
Dim nMaxIterations As Integer  ' maximum number of iterations in move loop

Dim iError As Integer          ' set to 1 upon error
Dim iCommand As Integer        ' command routine number to execute
Dim iAxis As Integer           ' axis number 
Dim fSpeed As Float            
Dim fDistance As Float         
Dim fPosition As Float

Dim fPos1 As Float
Dim fPos2 As Float
Dim fPos3 As Float

Dim fPosErr As Float                               ' position error after a move

Dim fMaxPosErr(0 To 2) As Float = { 0.005, 0.005, 0.005 } ' the maximum deviation from the requested move position

Dim fDefaultSpeed(0 To 2) As Float = { 0.1, 0.2, 0.05 }    ' default speeds for all axes
Dim fMaxSpeed(0 To 2) As Float     = { 0.2, 1., 0.1 }     ' max speeds in mm/s (1/20 wedge for axis 2)
Dim fMinSpeed(0 To 2) As Float     = { 0.01, 0.01, 0.0005 } ' min speeds in mm/s

Dim fSoftLimFwd(1 To 2) As Float = {  7.0,  2.45 }
Dim fSoftLimRev(1 To 2) As Float = { -7.0, -2.45 }

' ===============================================================================================
' Main Program execution loop
' ===============================================================================================

' Initialisation
iCodeVersion   = 5

COMMS(1)       = 0    ' reset iCommand
COMMS(5)       = 0    ' abort event handle, see Event COMMS5
COMMS(10)      = 0    ' nothing is requested
COMMS(20)      = 1    ' controller software status
COMMS(21)      = iCodeVersion 

nMaxIterations = 5     ' maximum number of iterations in move loop



Loop  
  ' check whether we need to execute a command
  iCommand = COMMS(1)
  iAxis    = COMMS(2)
  
  ' reinitialise iError
  iError = 0
  
  If iCommand <> 0 Then
  
    If iCommand = 1 Then       'Enable the selected axis
      Print "Enabling axis ", iAxis 
      DRIVEENABLE(iAxis) = 1
      
    ElseIf iCommand = 2 Then   'Disable the selected axis
      Print "Disabling axis ", iAxis
      DRIVEENABLE(iAxis) = 0
    
    ElseIf iCommand = 3 Then   'Get the encoder position
      Print "Axis ", iAxis, " encoder position : ", ENCODER(iAxis), " mm."
      COMMS(8) = ENCODER(iAxis)
      
    ElseIf iCommand = 4 Then   'Set the encoder position from COMMS(3)
      Print "Setting encoder on axis ", iAxis, " to ", COMMS(3), " mm."
      ENCODER(iAxis) = COMMS(3)
      COMMS(8) = ENCODER(iAxis)
      
    ElseIf iCommand = 5 Then   'Execute a relative move
      COMMS(10) = 1
      fDistance = COMMS(3)
      fSpeed    = COMMS(4)

      Print "Moving axis ", iAxis, " over ", fDistance, " mm, speed ", fSpeed, " mm/s."
      Move iAxis, fDistance, fSpeed
      Print "Done."
      COMMS(10) = 2
      
    ElseIf iCommand = 6 Then   'Execute an absolute move
      COMMS(10) = 1
      fPosition = COMMS(3)
      fSpeed    = COMMS(4)
      
      Print "Moving axis ", iAxis, " to position ", fPosition, " mm, speed ", fSpeed, " mm/s."
      MoveTo iAxis, fPosition, fSpeed
      Print "Done."
      COMMS(10) = 2
    
      
    ElseIf iCommand = 7 Then ' Execute the homing routine
      COMMS(10) = 1
      Print "Homing axis ", iAxis
      GoHome iAxis    
      Print "Done."
      COMMS(10) = 2
    
    ElseIf iCommand = 8 Then   'Execute a relative rotation
      COMMS(10) = 1
      fDistance = COMMS(3)
      fSpeed    = COMMS(4)

      Print "Rotating axis ", iAxis, " over ", fDistance, " degrees, speed ", fSpeed, " deg./s."
      Move iAxis, fDistance, fSpeed
      Print "Done."
      COMMS(10) = 2
      
    ElseIf iCommand = 6 Then   'Execute an absolute rotation
      COMMS(10) = 1
      fPosition = COMMS(3)
      fSpeed    = COMMS(4)
      
      Print "Rotating axis ", iAxis, " to position ", fPosition, " degrees, speed ", fSpeed, " deg./s."
      MoveTo iAxis, fPosition, fSpeed
      Print "Done."
      COMMS(10) = 2
    
    
    End If ' end of command selection
  
    ' resets the command number
    COMMS(1) = 0  
  
  End If ' We had a COMMS(1) <> 0, so executed the command selection

End Loop  

  
' ===============================================================================================
' The Startup block
' ===============================================================================================

Startup

' Stop motion and clear all errors
CANCELALL

' Drive enable output, CHECK WHETHER THIS IS HOOKED UP CORRECTLY !!!! 
DRIVEENABLEOUTPUT(0) = 12
DRIVEENABLEOUTPUT(1) = 12
DRIVEENABLEOUTPUT(2) = 12

' Set the limit switches, active when high on DIN 0,1,2,3 and all the
' rest, except for mechanical switch DIN 4, this is active when it goes low
INPUTACTIVELEVEL = 0xFFFEF

' Define the limit switches
LIMITREVERSEINPUT(1) = 0
LIMITFORWARDINPUT(1) = 1

LIMITREVERSEINPUT(2) = 3 ' note the flip since we changed the steppers' direction
LIMITFORWARDINPUT(2) = 2

' Define the error input, this is the mechanical limit switch on the horizontal stage
ERRORINPUT(1) = 4

' Setting the way MINT reacts to limit switch getting hit
LIMITMODE(1) = _emCRASH_STOP_DISABLE
LIMITMODE(2) = _emCRASH_STOP_DISABLE

' Setting soft limits, depending on where the encoder 0 was set before, these might 
' be crap, so use the GoHome routine to set them properly

SOFTLIMITMODE(1)  = _emCRASH_STOP
SOFTLIMITMODE(2)  = _emCRASH_STOP
'SOFTLIMITMODE(1)  = _emIGNORE
'SOFTLIMITMODE(2)  = _emIGNORE

SOFTLIMITFORWARD(1) = fSoftLimFwd(1)
SOFTLIMITREVERSE(1) = fSoftLimRev(1)

SOFTLIMITFORWARD(2) = fSoftLimFwd(2)
SOFTLIMITREVERSE(2) = fSoftLimRev(2)

' Setting the scalefactors, distances in mm, speeds in mm/s
SCALEFACTOR(0) = 200    ' rotation, xxx, 200 counts per rev
SCALEFACTOR(1) = 200    ' 1 mm pitch screw, 200 counts per rev
SCALEFACTOR(2) = 200    ' 1 mm pitch screw, 200 counts per rev

ENCODERSCALE(0) =  10000 ' 10000 counts per xxx
ENCODERSCALE(1) =  10000 ' 10000 counts per mm ( 0.1 micron res), neg.
ENCODERSCALE(2) =  10000 ' 10000 counts per mm ( 0.1 micron res)

' Setting the polarity of the stepper motors and the encoders
' this way we ensure positive direction is going north and 
' negative going south in horizontal plane, and positive is up and 
' negative is down for the vertical. Also the correct limit switches
' get hit, so the forward when moving forward ;)
ENCODERMODE(0) = 0                  ' polarity of rotational encoder is fine
ENCODERMODE(1) = _emCOUNT_DIRECTION ' reverse polarity for horizontal encoder
ENCODERMODE(2) = 0                  ' polarity of veritcal encoder is fine 

STEPPERMODE(0) = _stmNORMAL_DIRECTION  ' rotation is fine
STEPPERMODE(1) = _stmNORMAL_DIRECTION  ' horizontal is fine
STEPPERMODE(2) = _stmREVERSE_DIRECTION ' flip vertical motor polarity


' Set the default speed
SPEED(0) = fDefaultSpeed(0)
SPEED(1) = fDefaultSpeed(1)
SPEED(2) = fDefaultSpeed(2)

' Enable all drives at end of startup
DRIVEENABLE(0) = 1
DRIVEENABLE(1) = 1
DRIVEENABLE(2) = 1

End Startup
 
' ===============================================================================================
' Event Implementations
' ===============================================================================================
Event ONERROR
  iError=1
  ' Read the next error and handle accordingly
  ' Check first if we have a motion error and handle accordingly
  If ERRORREADNEXT(_egAXIS_ERROR, -1) Then 
    iAxis = ERRDATA(1) ' get the axis 
    
    Select Case ERRCODE
    
    Case _ecFWD_HARD_LIMIT
      ' Forward limit hit, jog backwards and clear it 
      ' Assuming a default action of ramp to a controlled halt
      ' Turn off limit detection, clear other axis error, slowly jog
      ' off limit, turn on limit detection again and bring axis to a halt
      Print "*** error ", ERRCODE, " : forward hard limit hit on axis ", iAxis, ", restoring..." 
      STOP(iAxis)
      Pause(IDLE(iAxis))
      LIMITMODE(iAxis) = _emIGNORE
      CANCEL(iAxis)            
      Pause(IDLE(iAxis))     
      DRIVEENABLE(iAxis) = 1
      JOG(iAxis) = -1 * fDefaultSpeed(iAxis) / 10. 
      Pause(!LIMITFORWARD(iAxis)) 
      LIMITMODE(iAxis) = _emCRASH_STOP_DISABLE
      STOP(iAxis)
      Print "*** error ", ERRCODE, " : axis ", iAxis, " restored."
      Exit Event
    
    Case _ecREV_HARD_LIMIT
      ' Backward limit hit, jog backwards and clear it !
      Print "*** error ", ERRCODE, " : reverse hard limit hit on axis ", iAxis, ", restoring..." 
      STOP(iAxis)
      Pause(IDLE(iAxis))
      LIMITMODE(iAxis) = _emIGNORE
      CANCEL(iAxis)            
      Pause(IDLE(iAxis))     
      DRIVEENABLE(iAxis) = 1  
      JOG(iAxis) = fDefaultSpeed(iAxis) / 10. 
      Pause(!LIMITREVERSE(iAxis)) 
      LIMITMODE(iAxis) = _emCRASH_STOP_DISABLE
      STOP(iAxis)
      Print "*** error ", ERRCODE, " : axis ", iAxis, " restored."
      Exit Event

   Case _ecFWD_SOFT_LIMIT
      Print "*** error ", ERRCODE, " : forward soft limit hit on axis ", iAxis, ", restoring..." 
      STOP(iAxis)
      Pause(IDLE(iAxis))
      CANCEL(iAxis)            
      Pause(IDLE(iAxis))     
      DRIVEENABLE(iAxis) = 1
      JOG(iAxis) = -1 * fDefaultSpeed(iAxis) / 10. 
      Pause(POS(iAxis)<SOFTLIMITFORWARD(iAxis))
      STOP(iAxis)
      Print "*** error ", ERRCODE, " : axis ", iAxis, " restored."
      Exit Event
    
    Case _ecREV_SOFT_LIMIT
      Print "*** error ", ERRCODE, " : reverse soft limit hit on axis ", iAxis, ", restoring..." 
      STOP(iAxis)
      Pause(IDLE(iAxis))     
      CANCEL(iAxis)            
      Pause(IDLE(iAxis))     
      DRIVEENABLE(iAxis) = 1  
      JOG(iAxis) = fDefaultSpeed(iAxis) / 10. 
      Pause(POS(iAxis)>SOFTLIMITREVERSE(iAxis)) 
      STOP(iAxis)
      Print "*** error ", ERRCODE, " : axis ", iAxis, " restored."
      Exit Event

    Case _ecERROR_INPUT
      ' Mechanical limit switch hit, power to motor interrupted, need an 
      ' access. This is a severe failure condition of the motion control system
      Print "*** FATAL !! mechanical hard limit hit, aborting..."
      Print "*** you will need to manually drive back the stages"
      ABORT
      End

    Case _ecABORT
      ' Software abort...
      Print "*** error ", ERRCODE, " : Software ABORT"
      End ' All axes crash stopped and disabled
    
    Case Else
      ' Handle all other axis warnings and errors
      Print "*** error ", ERRCODE, " occurred."
      
    End Select

    Print "Axis Error code: ", ERRCODE
    Print "Description: ", ERRSTRING
    Print "Axis: ", ERRDATA(1)
    Print "Time: ", ERRTIME
    End    
    
  End If ' Motion Error

  ' Was not a motion error
  If ERRORREADNEXT(_egALL, -1) Then
    Print "*** error ", ERRCODE, " occured."
    Print "    Description: ", ERRSTRING
    Print "    Axis (if applicable): ", ERRDATA(1)
    Print "    Line number (if applicable): ", ERRLINE
    Print "    Time: ", ERRTIME
    Print "*** aborting program execution"
    ' Stop all motion and exit program
    ABORT
    End
  End If 
  
End Event 'End of error handling event
 
 
Event COMMS5
  ' COMMS(5) is modified, this indicates the user wants to stop all motion
  ' let's to this then :)
  If COMMS(5) = 1 Then
    Print "Comms5 event called, motion on current axis stopped by user."
    STOP(iAxis)   ' stop motion on current axis
  Else 
    Print "Comms5 event called, motion on all axes cancelled by user."
    CANCEL(1,2)   ' stop motion on all axes, clear errors
  End If
  iError   = 1 ' to interrupt a possible loop we're having
  COMMS(5) = 0 ' reset the comms(5) element
End Event
 
' ===============================================================================================
' Subroutine Implementations
' ===============================================================================================

Sub Move( iAxis As Integer, fDistance As Float, fSpeed As Float )
  ' Executes a relative move on the specified axis over fDistance with speed
  ' fSpeed, just sets the distance, speed and executes the moveto command
  
  ' This routine gets executed by setting COMMS(1) = 5
  
  MoveTo iAxis, ( ENCODER(iAxis) + fDistance ), fSpeed
  
End Sub


Sub MoveTo( iAxis As Integer, fPosition As Float, fSpeed As Float )
  ' Moves to a specific encoder position, with an iteration with some
  ' encoder feedback as if it were a closed loop
  
  ' This routine gets executed by setting COMMS(1) = 6
  
  If ( fSpeed > fMaxSpeed(iAxis) ) Then
    fSpeed = fMaxSpeed(iAxis)
    Print "*** warning: speed out of range, setting to max speed of ", fMaxSpeed(iAxis)
  End If
  
  If ( fSpeed < fMinSpeed(iAxis) ) Then
    fSpeed  = fMinSpeed(iAxis)
    Print "*** warning: speed out of range, setting to min speed of ", fMinSpeed(iAxis)
  End If
 
  ' set the speed
  SPEED(iAxis) = fSpeed
  nIteration = 0

  Print "  moving axis ", iAxis, " from ", ENCODER(iAxis), " to ", fPosition
  Repeat 
    nIteration = nIteration + 1 'count the iterations
    MOVER(iAxis) = fPosition - ENCODER(iAxis)
    GO(iAxis)
    Pause( IDLE(iAxis) | iError ) ' wait for the move to complete
    Wait(100) ' wait a bit for axis to settle
        
    ' reduce the speed by factor of 10 for second and more iterations...
    SPEED(iAxis) = fSpeed / 10

    ' Calculate the position error after this move
    fPosErr = Abs( fPosition - ENCODER(iAxis) )
    Print "   [", nIteration, "] encoder.",iAxis," = ", ENCODER(iAxis), " residual = ", fPosErr 
    
  Until (  ( fPosErr < fMaxPosErr(iAxis) )  | ( nIteration >= nMaxIterations ) | iError )
  Print "  move finished in ", nIteration, " iterations, residual : ", Abs(fPosition - ENCODER(iAxis))
  
  ' If iError was set and the loop interrupted by it, we need to clear it. 
  ' The error will have been cleared already
  iError = 0
  
  ' set the position on the axis to the encoder position to be sure we
  ' don't mess up software limits in the long run
  POS(iAxis) = ENCODER(iAxis)
End Sub



Sub GoHome( iAxis As Integer )
  ' Sort of homing routine based upon the limit switches due to the absense of
  ' a homing index mark.
  
  ' Algorithm should be something like... 
  ' jog until end switch is hit, record encoder position, 
  ' jog other way untill end switch is hit, record the other position 
  ' MoveTo the center
  ' reset the encoder position
  
  ' later on one needs to switch off software limits here
  ' and rely on hardware limits
  
  SOFTLIMITMODE(iAxis) = _emIGNORE
  
  Print "  finding positive limit"
  JOG(iAxis) = fDefaultSpeed(iAxis)
  Pause(IDLE(iAxis) | iError)
  fPos1 = ENCODER(iAxis)
  iError = 0 ' reset error code
  Print "  first edge of motion at ", fPos1
  
  Print "  finding negative limit"
  JOG(iAxis) = -fDefaultSpeed(iAxis)
  Pause(IDLE(iAxis) | iError)
  iError = 0 ' reset error code
  fPos2 = ENCODER(iAxis)
  Print "  second edge of motion at ", fPos2
  
  Print "  homing to ", 0.5 * ( fPos1 + fPos2 )
  MoveTo iAxis, 0.5 * (fPos1 + fPos2), fDefaultSpeed(iAxis)
  
  ' Reset the encoders and the position
  ENCODER(iAxis) = 0.
  POS(iAxis)     = 0.
  
  ' switch software limits back on, possibly reset them even...
  SOFTLIMITMODE(iAxis)    = _emCRASH_STOP_DISABLE 
  SOFTLIMITFORWARD(iAxis) = fSoftLimFwd(iAxis)
  SOFTLIMITREVERSE(iAxis) = fSoftLimRev(iAxis)
End Sub


' ============================================ all done :) ======================================