SimulationCore2/Equipments/TopDrive/TopDrive_Solver.f90

subroutine TopDrive_Solver
    
  
    Use TopDrive_VARIABLES
    Use CTdsConnectionModesEnumVariables
    
	IMPLICIT NONE
    
    
    
    TDS%TDS_wOld = ((pi*TDS%Speed)/30.d0)          ![rad/s]
    
    
!******************************************************************
   
    
    if ( TDS%PowerState==-1 .or. TDS%OldPowerMode==-1 ) then  !FWD
        
        if ( TDS%DrillTorqueState==0 .and. Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then  ! TdsMu_SPINE
            TDS%OperationFaultLed = 0
            TDS%Speed             = 0.d0              !Speed [RPM]
            TDS%RpmGaugeOutput    = 30.d0
            TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
            TDS%SoundRPM          = 30
            TDS%Torque            = 0.d0              ![ft.lbf]
            TDS%TorqueGaugeOutput = 0.d0
            TDS%OldPowerMode      = -1
        else if ( TDS%DrillTorqueState==0 .and. Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then  ! TdsMu_SPINE
            TDS%OperationFaultLed = 0
            TDS%Speed             = 0.d0              !Speed [RPM]
            TDS%RpmGaugeOutput    = TDS%Speed
            TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
            TDS%SoundRPM          = 0
            TDS%Torque            = 0.d0              ![ft.lbf]
            TDS%TorqueGaugeOutput = 1000.d0
            TDS%OldPowerMode      = -1
        else if ( TDS%DrillTorqueState==1 ) then      ! TdsMu_Torque
            if ( TDS%RpmKnob>0. ) then
                TDS%OperationFaultLed = 1
                TDS%Speed             = 0.d0          !Speed [RPM]
                TDS%RpmGaugeOutput    = TDS%Speed
                TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
                TDS%SoundRPM          = 0
                TDS%Torque            = 0.d0          ![ft.lbf]
                TDS%TorqueGaugeOutput = 0.d0          ![ft.lbf]
                TDS%OldPowerMode      = -1
            else
                TDS%OperationFaultLed = 0
                if ( Get_TdsConnectionModes()==TDS_CONNECTION_STRING ) then
                    TDS%Speed             = 0.d0         !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0         ![ft.lbf]
                    TDS%TorqueGaugeOutput = max( 1000.d0,((TDS%TorqueLimitKnob/10.d0)*60000.d0) )   ![ft.lbf]  ????
                    TDS%OldPowerMode      = -1
                else if ( Get_TdsConnectionModes()==Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then
                    TDS%Speed             = 0.d0         !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0      ![ft.lbf]
                    TDS%TorqueGaugeOutput = 0.d0      ![ft.lbf]
                    TDS%OldPowerMode      = -1
                else if ( Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then
                    TDS%Speed             = 0.d0      !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0      ![ft.lbf]
                    TDS%TorqueGaugeOutput = 1000.d0   ![ft.lbf]
                    TDS%OldPowerMode      = -1
                end if
            end if
        else if ( TDS%DrillTorqueState==-1 ) then     ! TdsMu_DRILL
            if ( Get_TdsConnectionModes()==TDS_CONNECTION_STRING ) then
                TDS%OperationFaultLed = 0
                TDS%Speed             = TDS%N_ref/TDS%Conv_Ratio         !Speed [RPM]
                TDS%Speed             = min(TDS%Speed,TDS%MaxRPM)
                TDS%RpmGaugeOutput    = TDS%Speed
                TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
                TDS%SoundRPM          = INT(TDS%Speed)
                TDS%Torque            = ( ((TDS%J_coef+TDS%String_JCoef)*(((((pi*TDS%N_ref)/30.d0)/TDS%Conv_Ratio)-TDS%TDS_wOld)/TDS%time_step))+(TDS%String_Torque) )*0.73756215d0   ![N.m]*0.73756215 = [ft.lbf]
                TDS%Torque            = min(TDS%Torque,TDS%MaxTorque)    ! [ft.lbf]
                TDS%TorqueGaugeOutput = TDS%Torque
                TDS%OldPowerMode      = -1
            else if ( Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then
                TDS%OperationFaultLed = 0
                TDS%Speed             = 0.0d0
                TDS%RpmGaugeOutput    = TDS%N_ref/TDS%Conv_Ratio    !Speed [RPM]
                TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
                TDS%SoundRPM          = INT(TDS%N_ref/TDS%Conv_Ratio)
                TDS%Torque            = 0.0d0
                TDS%TorqueGaugeOutput = 0.d0                        ![ft.lbf]
                TDS%OldPowerMode      = -1
            else if ( Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then
                if ( TDS%RpmKnob>0. ) then
                    TDS%OperationFaultLed = 1
                    TDS%Speed             = 0.d0      !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0      ![ft.lbf]
                    TDS%TorqueGaugeOutput = 0.d0      ![ft.lbf]
                    TDS%OldPowerMode     = -1
                else
                    TDS%OperationFaultLed = 0
                    TDS%Speed             = 0.d0      !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0      ![ft.lbf]
                    TDS%TorqueGaugeOutput = 0.d0      ![ft.lbf]
                    TDS%OldPowerMode      = -1
                end if
            end if
        else
            TDS%OperationFaultLed = 0
            TDS%Speed             = 0.d0      !Speed [RPM]
            TDS%RpmGaugeOutput    = TDS%Speed
            TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
            TDS%SoundRPM          = 0
            TDS%Torque            = 0.d0      ![ft.lbf]
            TDS%TorqueGaugeOutput = 0.d0      ![ft.lbf]
            TDS%OldPowerMode      = -1
        end if
                
    else if ( TDS%PowerState==1 .or. TDS%OldPowerMode==1 ) then  !REV
     
        if ( TDS%DrillTorqueState==0 .and. Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then  ! TdsMu_SPINE
            TDS%OperationFaultLed = 0
            TDS%Speed             = 0.d0            !Speed [RPM]
            TDS%RpmGaugeOutput    = 30.d0
            TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
            TDS%SoundRPM          = 30
            TDS%Torque            = 0.d0            ![ft.lbf]
            TDS%TorqueGaugeOutput = 0.d0
            TDS%OldPowerMode      = 1
        else if ( TDS%DrillTorqueState==0 .and. Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then  ! TdsMu_SPINE
            TDS%OperationFaultLed = 0
            TDS%Speed             = 0.d0            !Speed [RPM]
            TDS%RpmGaugeOutput    = 30.d0
            TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
            TDS%SoundRPM          = 0
            TDS%Torque            = 0.d0            ![ft.lbf]
            TDS%TorqueGaugeOutput = 1000.d0
            TDS%OldPowerMode      = 1
        else if ( TDS%DrillTorqueState==1 ) then   ! TdsMu_Torque
            if ( TDS%RpmKnob>0.d0 ) then
                TDS%OperationFaultLed = 1
                TDS%Speed             = 0.d0       !Speed [RPM]
                TDS%RpmGaugeOutput    = TDS%Speed
                TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
                TDS%SoundRPM          = 0
                TDS%Torque            = 0.d0       ![ft.lbf]
                TDS%TorqueGaugeOutput = 0.d0       ![ft.lbf]
                TDS%OldPowerMode      = 1
            else
                TDS%OperationFaultLed = 0
                if ( Get_TdsConnectionModes()==TDS_CONNECTION_STRING ) then
                    TDS%Speed             = 0.d0      !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0      ![ft.lbf]
                    TDS%TorqueGaugeOutput = 2000.d0
                    TDS%OldPowerMode      = 1
                else if ( Get_TdsConnectionModes()==Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then
                    TDS%Speed             = 0.d0      !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0      ![ft.lbf]
                    TDS%TorqueGaugeOutput = 0.d0      ![ft.lbf]
                    TDS%OldPowerMode      = 1
                else if ( Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then
                    TDS%Speed             = 0.d0      !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0      ![ft.lbf]
                    TDS%TorqueGaugeOutput = 1000.d0   ![ft.lbf]
                    TDS%OldPowerMode      = -1
                end if
            end if
        else if ( TDS%DrillTorqueState==-1 ) then     ! TdsMu_DRILL
            if ( Get_TdsConnectionModes()==TDS_CONNECTION_STRING ) then
                TDS%OperationFaultLed = 0
                TDS%Speed             = TDS%N_ref/TDS%Conv_Ratio        !Speed [RPM]
                TDS%Speed             = min(TDS%Speed,TDS%MaxRPM)
                TDS%RpmGaugeOutput    = TDS%Speed
                TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
                TDS%SoundRPM          = INT(TDS%Speed)
                TDS%Torque            = ( ((TDS%J_coef+TDS%String_JCoef)*(((((pi*TDS%N_ref)/30.d0)/TDS%Conv_Ratio)-TDS%TDS_wOld)/TDS%time_step))+(TDS%String_Torque) )*0.73756215d0   ![N.m]*0.73756215 = [ft.lbf]
                TDS%Torque            = min(TDS%Torque,TDS%MaxTorque)    ! [ft.lbf]
                TDS%TorqueGaugeOutput = TDS%Torque
                TDS%OldPowerMode      = 1
            else if ( Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then
                TDS%OperationFaultLed = 0
                TDS%Speed             = 0.0d0
                TDS%RpmGaugeOutput    = TDS%N_ref/TDS%Conv_Ratio    !Speed [RPM]
                TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
                TDS%SoundRPM          = INT(TDS%N_ref/TDS%Conv_Ratio)
                TDS%Torque            = 0.0d0
                TDS%TorqueGaugeOutput = 0.d0                        ![ft.lbf]
                TDS%OldPowerMode      = 1
            else if ( Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then
                if ( TDS%RpmKnob>0. ) then
                    TDS%OperationFaultLed = 1
                    TDS%Speed             = 0.d0      !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0      ![ft.lbf]
                    TDS%TorqueGaugeOutput = 0.d0      ![ft.lbf]
                    TDS%OldPowerMode      = 1
                else
                    TDS%OperationFaultLed = 0
                    TDS%Speed             = 0.d0      !Speed [RPM]
                    TDS%RpmGaugeOutput    = TDS%Speed
                    TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
                    TDS%SoundRPM          = 0
                    TDS%Torque            = 0.d0      ![ft.lbf]
                    TDS%TorqueGaugeOutput = 0.d0      ![ft.lbf]
                    TDS%OldPowerMode      = 1
                end if
            end if
        else
            TDS%OperationFaultLed = 0
            TDS%Speed             = 0.d0      !Speed [RPM]
            TDS%RpmGaugeOutput    = TDS%Speed
            TDS%RPMUnityOutput    = -TDS%RpmGaugeOutput
            TDS%SoundRPM          = 0
            TDS%Torque            = 0.d0      ![ft.lbf]
            TDS%TorqueGaugeOutput = 0.d0      ![ft.lbf]
            TDS%OldPowerMode      = 1
        end if
        
    end if
    
    
    
!******************************************************************
    
   
    
    
END subroutine