'==================================================
' IRTF Dome Servo Program
'
'===================================================
Define DVEL_MAX = 7792  ' 77920 c/s is 1141 RPM. Dome speed =  2.0 deg/sec

Define RMODE_IDLE      = 0 ' values for COMMS(CINX_RMODE)
Define RMODE_JOG       = 1  

' Define CINX_ indexes into the COMMS array
Define CINX_RMODE      = 1
Define CINX_RVEL       = 2
Define CINX_MAIN_SEC   = 6
Define CINX_EPL_ERRROR = 7
Define CINX_CLOOP_MS   = 10

Define CINX_AO_ASW     = 11
Define CINX_AO_MODE    = 12
Define CINX_AO_TEMP0   = 13
Define CINX_A0_TEMP1   = 14
Define CINX_A0_VEL     = 15
Define CINX_A0_CURRENT = 16

Define CINX_A1_ASW     = 21
Define CINX_A1_MODE    = 22
Define CINX_A1_TEMP0   = 23
Define CINX_A1_TEMP1   = 24
Define CINX_A1_VEL     = 25
Define CINX_A1_CURRENT = 26

Define CINX_A2_ASW     = 31
Define CINX_A2_MODE    = 32
Define CINX_A2_TEMP0   = 33
Define CINX_A2_TEMP1   = 34
Define CINX_A2_VEL     = 35
Define CINX_A2_CURRENT = 36
 

'--------------------------------
'  Main Parent Task
'--------------------------------
Dim EPL_Nodes(2) As Integer = { 100, 101 } ' we have 2 Control nodes
Dim EPL_Axis(2) As Integer = { 1, 2 }      ' control node are axis 1,2

Print "Stating main ", Terminal

' start commsTask - write data to COMMS
Run CommsTask
TaskPriority( CommsTask, 6) ' lower it from the default of 10

'check EPL devices
Repeat
  COMMSINTEGER(CINX_EPL_ERRROR) = init_EPL (EPL_Nodes, EPL_Axis)
Until COMMSINTEGER(CINX_EPL_ERRROR) = 0

Loop
     COMMSINTEGER(CINX_MAIN_SEC) = Time/ 1000  ' uptime counter in seconds.
End Loop
End
'-----------------------------------
' CommsTask - puts data in comm array
'-----------------------------------
Task CommsTask
  Dim t1 As Integer
  Loop
    t1 = Time
    '---------------
    ' Axis 0
    COMMSINTEGER(CINX_AO_ASW) = AXISSTATUSWORD(0)
    COMMSINTEGER(CINX_AO_MODE) = AXISMODE(0)
    COMMS(CINX_AO_TEMP0) = TEMPERATURE(0)
    COMMS(CINX_A0_TEMP1) = TEMPERATURE(1)
    COMMS(CINX_A0_VEL) = VEL(0)
    COMMS(CINX_A0_CURRENT) = CURRENTMEAS(0,0)
    '---------------
    ' Axis 1
    COMMSINTEGER(CINX_A1_ASW) = AXISSTATUSWORD(1)
    COMMSINTEGER(CINX_A1_MODE) = AXISMODE(1)
    'COMMS(23) - temperature via ???
    'COMMS(24) - temperature via ???
    COMMS(CINX_A1_VEL) = VEL(1)
    'COMMS(26) = CURRENTMEAS(1,0) - need to setup process data config?
    '------------------------
    ' Axis 2
    COMMSINTEGER(CINX_A2_ASW) = AXISSTATUSWORD(2)
    COMMSINTEGER(CINX_A2_MODE) = AXISMODE(2)
    'COMMS(23) - temperature via ???
    'COMMS(24) - temperature via ???
    COMMS(CINX_A2_VEL) = VEL(1)
    'COMMS(36) = CURRENTMEAS(2,0)
    
    COMMSINTEGER(CINX_CLOOP_MS) = Time-t1   ' time it takes to run the loop
  End Loop  
End Task
'--------------------------------------------------------------------------
' Init_EPL - check that the EPL network is OK.
' Parameters
'    nEPLNodes() - array of integer ID's the EPL nodes
' Return code to indicate an error.
'  -1 = EPL bus no Cyclic (not in EPL mode).
'  -2 = EPL nodes NOT  alive.
'  -3 = EPL nodes NOT operational
'  -4 = Remote PDO NOT Valid
'  -5 = Remote axis did NOT initialize.
'  -6 = Remote axis NOT under EPL control (_aswREMOTE not set)
'--------------------------------------------------------------------------

Function init_EPL(nEPLNodes() As Integer, nEPLAxes() As Integer) As Integer

  Const _btINITIALISED = (_aswSWITCH_ON_DISABLED Or _aswREADY_TO_SWITCH_ON _
                   Or _aswSWITCHED_ON Or _aswOPERATION_ENABLED Or _aswFAULT)

  Dim n As Integer
  Dim bNodesOK As Integer

  ' check Ethernet Bus state for normal EPL operation is _bsCYCLIC
  Time = 0
  Loop
    If BUSSTATE(_busETHERNET) = _bsCYCLIC Then
      Exit Loop
    End If

    If Time > 10000 Then
      init_EPL = -1
      Exit Function
    End If
      End Loop

  ' insure all node on the bus are alive.
  Time = 0
  Loop
    bNodesOK = _true
    For n = 1 To UBound(nEPLNodes)
      If NODELIVE(_busETHERNET,nEPLNodes(n)) = _false Then
        bNodesOK = _false
        Exit For
      End If
    Next

    If bNodesOK = _true Then
      Exit Loop
    End If

    If Time > 10000 Then
      init_EPL = -2
      Exit Function
    End If
  End Loop

  'Wait for the nodes to go into the operational state, once the nodes
  'are in the operational state SDO communication is avaliable
  Time = 0
  Loop
    bNodesOK = _true
    For n = 1 To UBound(nEPLNodes)
      If REMOTESTATUS(_busETHERNET,nEPLNodes(n)) <> _rsEPL_OPERATIONAL Then
        bNodesOK = _false
                Exit For
      End If
    Next n

    If bNodesOK = _true Then
      Exit Loop
    End If

    If Time > 10000 Then
      init_EPL = -3
      Exit Function
    End If
  End Loop

  'Wait for the data valid flag in the PDO messages to be set, this
  'indicates that the nodes are talking correctly on the network
  Time = 0
  Loop
    bNodesOK = _true
    For n = 1 To UBound(nEPLNodes)
      If REMOTEPDOVALID(_busETHERNET, nEPLNodes(n)) <> _true Then
        bNodesOK = _false
        Exit For
      End If
    Next n

    If bNodesOK = _true Then
      Exit Loop
    End If

    If Time > 10000 Then
      init_EPL = -4
      Exit Function
    End If
  End Loop

  ' Waiting for axes to initialise...
  'Wait for the DS402 state machine to exit the 'Not Ready to Switch On'
  'state, this indicates that the drives have completed their initialisation
  Time = 0
  Loop
    bNodesOK = _true
    For n = 1 To UBound(nEPLAxes)
      If (AXISSTATUSWORD(nEPLAxes(n)) & _btINITIALISED) = 0 Then
        bNodesOK = _false
        Exit For
      End If
    Next n

    If bNodesOK = _true Then
      Exit Loop
    End If

    If Time > 10000 Then
      init_EPL = -5
      Exit Function
    End If
  End Loop

  'Wait for the 'remote' bit in the status word to be set.  This indicates
  'that the axis is under EPL control
  Time = 0
  Loop
    bNodesOK = _true
    For n = 1 To UBound(nEPLAxes)
      If (AXISSTATUSWORD(nEPLAxes(n)) & _aswREMOTE) = 0 Then
        bNodesOK = _false
        Exit For
      End If
    Next n

    If bNodesOK = _true Then
      Exit Loop
    End If

    If Time > 10000 Then
      init_EPL = -6
      Exit Function
    End If
  End Loop

  ' ALL OK, return 0
  init_EPL = 0
End Function
'---------------------------------------
' clip_range() - function to clip an integer between Min..Max
'---------------------------------------
Function clip_range( ByVal i As Integer, ByVal imin As Integer, ByVal imax As Integer) As Integer
  If( i < imin ) Then i = imin
  If (i > imax ) Then  i = imax    
  clip_range = i
End Function
'-----------------------------------
' Startup - Executes at the begining of the program
'-----------------------------------
Startup
  Dim i As Integer
  'Clear Errors
  CANCEL(0)
  Pause IDLE(0)
  'set comms data to 0.
  For i =  1 To 39
    COMMSINTEGER(i) = 0
  Next i
  COMMSINTEGER(CINX_RMODE) = RMODE_IDLE
  
End Startup