'==================================================
' IRTF Dome Servo Program
'
'===================================================
Option Auto 1

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

' Event COMMS1 commands
Define CMD_ERROR0      = 0  ' Error zero
Define CMD_DENB_OFF    = 1  ' DriveEnable Off
Define CMD_DENG_ON     = 2  ' DriveEnable On
Define CMD_VEL_ADJ     = 3  ' Adjust Jog Velocity
Define CMD_ISREADY     = 10 ' Ready for next command.

' Define CINX_ indexes into the COMMS array
Define CINX_CMD         = 1
Define CINX_RVEL        = 2
Define CINX_EPL_ERRROR  = 6
Define CINX_MAIN_SEC    = 9
Define CINX_COMMS_UPDATE_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_CURRENT  = 15
Define CINX_A0_BRAKE    = 16
Define CINX_A0_VEL      = 19
Define CINX_A0_VELDEMAND= 20

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

Define CINX_A2_ASW      = 31
Define CINX_A2_MODE     = 32
Define CINX_A2_TEMP0    = 33
Define CINX_A2_TEMP1    = 34
Define CINX_A2_CURRENT  = 35
Define CINX_A2_BRAKE    = 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 "

'check EPL devices
Repeat
  COMMSINTEGER(CINX_EPL_ERRROR) = init_EPL (EPL_Nodes, EPL_Axis)
Until COMMSINTEGER(CINX_EPL_ERRROR) = 0
' After EPL is OK, set
COMMSINTEGER(CINX_CMD) = CMD_ISREADY
Loop
  Comms_update()
  COMMSINTEGER(CINX_MAIN_SEC) = Time/ 1000  ' uptime counter in seconds.
End Loop
End
'-----------------------------------
' Comms_update - puts data in comm array
'-----------------------------------
Sub Comms_update()
  Dim t1 As Integer
  
  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_CURRENT) = CURRENTMEAS(0,0)
  COMMSINTEGER(CINX_A0_BRAKE) = MOTORBRAKESTATUS(0)
  COMMS(CINX_A0_VEL) = VEL(0)
  COMMS(CINX_A0_VELDEMAND) = VELDEMAND(0)
  '---------------
  ' Axis 1
  COMMSINTEGER(CINX_A1_ASW) = AXISSTATUSWORD(1)
  COMMSINTEGER(CINX_A1_MODE) = AXISMODE(1)
  'COMMS(CINX_A1_TEMP0) - temperature via ???
  'COMMS(CINX_A1_TEMP1) - temperature via ???
  COMMS(CINX_A1_CURRENT) = CURRENTMEAS(1,0)
  COMMSINTEGER(CINX_A1_BRAKE) = MOTORBRAKESTATUS(1)
  '------------------------
  ' Axis 2
  COMMSINTEGER(CINX_A2_ASW) = AXISSTATUSWORD(2)
  COMMSINTEGER(CINX_A2_MODE) = AXISMODE(2)
  'COMMS(CINX_A2_TEMP0) - temperature via ???
  'COMMS(CINX_A2_TEMP1) - temperature via ???
   COMMS(CINX_A2_CURRENT) = CURRENTMEAS(2,0)
   COMMSINTEGER(CINX_A2_BRAKE) = MOTORBRAKESTATUS(2)
    
   COMMSINTEGER(CINX_COMMS_UPDATE_MS) = Time-t1   ' time it takes to run the loop
   
End Sub
'--------------------------------------------------------------------------
' 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
  
  'STOP(0, 1, 2)
  'Pause IDLE(0)
  'set comms data to 0.
  For i =  1 To 39
    COMMSINTEGER(i) = 0
  Next i
End Startup
'-----------------------------------------------
' comms1 event - use COMM event to command the controller
'-----------------------------------------------
Event COMMS1
  ' comms 1 value tell what us what to do
  Select COMMSINTEGER(1)
    Case CMD_DENB_OFF
       COMMSINTEGER(CINX_RVEL) = 0 ' requested vle
       DRIVEENABLE(0) = 0          ' DRIVEENABLE off
    Case CMD_DENG_ON
       COMMSINTEGER(CINX_RVEL) = 0 ' requested vel
       DRIVEENABLE(0) = 1          ' DRIVEENABLE on
       Wait(1000)   ' can proceed until brakes are off.
       VELREF(0) = 0
    Case CMD_VEL_ADJ
       VELREF(0) = COMMSINTEGER(2)
    Case Else
       Print "CommsEvent else - NOP"
  End Select
  
  'Indicated event is done. Ready for next command.
  COMMSINTEGER(1) = CMD_ISREADY
End Event