SimulationCore2/Equipments/TopDrive/TopDrive_Solver.f90

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