SimulationCore2/Equipments/TopDrive/TopDrive_Solver.f90

subroutine TopDrive_Solver
    
    !use CDrillingConsoleVariables
    !use CDataDisplayConsoleVariables
    ! use CSimulationVariables
    use CDrillWatchVariables
    use TopDrive_VARIABLES
    use CSounds
    use CTopDrivePanelVariables
    use CTdsConnectionModesEnumVariables
    use equipments_PowerLimit
    
	IMPLICIT NONE
    
    REAL :: const
    
    
    !TDS%TracTorque = TDS%String_Torque/TDS%Conv_Ratio/TDS%Mech_Efficiency
    TDS_wOld = TDS%w_new/TDS%Conv_Ratio
    CALL TopDrive_Traction_Motor
    
    
    !if (TDS%N_ref<=0.) then
    !    Call TopDrive_OffMode
    !end if
    
    
    
!******************************************************************
    !IF (TDS%ia_new<=1150.) THEN
        TDS%fii = 6.3304d-3*1150.d0
    !ELSE IF (TDS%ia_new>1150.) THEN
    !    TDS%fii = 2.8571d-7*(TDS%ia_new-1150.)+7.28
    !END IF
    
    TDS%Te = TDS%fii*TDS%ia_new
    const  = TDS%J_coef+(TDS%String_JCoef/(TDS%Mech_Efficiency*TDS%Conv_Ratio))
    TDS%dw = (TDS%Te-TDS%TL)/(const)
    
    
    
    
    
    
    if ( TopDrivePanel%TopDriveTdsPowerState==-1 .or. TDS_OldPowerMode==-1 ) then  !FWD
        
        if ( TopDrivePanel%TopDriveDrillTorqueState==0 .and. Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then  ! TdsMu_SPINE
            TopDrivePanel%TopDriveOperationFaultLed = 0
            TDS%Speed = 0.d0              !Speed [RPM]
            TopDrivePanel%TopDriveRpmGauge = 30.d0
            TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
            TDS%SoundRPM = 30
            !Call SetSound( TDS%SoundRPM )
            TDS%Torque           = 0.d0   ![ft.lbf]
            TopDrivePanel%TopDriveTorqueGauge  = 0.d0
            TDS_OldPowerMode     = -1
        else if ( TopDrivePanel%TopDriveDrillTorqueState==0 .and. Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then  ! TdsMu_SPINE
            TopDrivePanel%TopDriveOperationFaultLed = 0
            TDS%Speed = 0.d0              !Speed [RPM]
            TopDrivePanel%TopDriveRpmGauge = TDS%Speed
            TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
            TDS%SoundRPM = 0
            !Call SetSound( TDS%SoundRPM )
            TDS%Torque           = 0.d0   ![ft.lbf]
            TopDrivePanel%TopDriveTorqueGauge  = 1000.d0
            TDS_OldPowerMode     = -1
        else if ( TopDrivePanel%TopDriveDrillTorqueState==1 ) then  ! TdsMu_Torque
            if ( TopDrivePanel%RpmKnob>0. ) then
                TopDrivePanel%TopDriveOperationFaultLed = 1
                TDS%Speed = 0.d0                 !Speed [RPM]
                TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
                TDS%SoundRPM = 0
                !Call SetSound( TDS%SoundRPM )
                TDS%Torque           = 0.d0      ![ft.lbf]
                TopDrivePanel%TopDriveTorqueGauge  = 0.d0      ![ft.lbf]
                TDS_OldPowerMode     = -1
            else
                TopDrivePanel%TopDriveOperationFaultLed = 0
                if ( Get_TdsConnectionModes()==TDS_CONNECTION_STRING ) then
                    TDS%Speed = 0.d0                 !Speed [RPM]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = max( 1000.d0,((TopDrivePanel%TopDriveTorqueLimitKnob/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]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = 0.d0      ![ft.lbf]
                    TDS_OldPowerMode     = -1
                else if ( Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then
                    TDS%Speed = 0.d0                 !Speed [RPM]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = 1000.d0   ![ft.lbf]
                    TDS_OldPowerMode     = -1
                end if
            end if
        else if ( TopDrivePanel%TopDriveDrillTorqueState==-1 ) then  ! TdsMu_DRILL
            if ( Get_TdsConnectionModes()==TDS_CONNECTION_STRING ) then
                TopDrivePanel%TopDriveOperationFaultLed = 0
                TDS%Speed         = (30.d0*TDS%w_new/pi)/TDS%Conv_Ratio     !Speed [RPM]
                TopDrivePanel%TopDriveRpmGauge  = TDS%Speed
                TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
                TDS%SoundRPM      = INT(TDS%Speed)
                !Call SetSound( TDS%SoundRPM )
                TDS%Torque         = ( ((TDS%J_coef+TDS%String_JCoef)*(((TDS%w_new/TDS%Conv_Ratio)-TDS_wOld)/TDS%time_step))+(TDS%String_Torque) )*0.73756215   ![N.m]*0.73756215 = [ft.lbf]
                TopDrivePanel%TopDriveTorqueGauge= TDS%Torque
                TDS_OldPowerMode   = -1
            else if ( Get_TdsConnectionModes()==Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then
                TopDrivePanel%TopDriveOperationFaultLed = 0
                TDS%Speed = 0.0d0
                TopDrivePanel%TopDriveRpmGauge  = ((30.d0*TDS%w_new/pi)/TDS%Conv_Ratio)    !Speed [RPM]
                TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
                TDS%SoundRPM      = INT((30.d0*TDS%w_new/pi)/TDS%Conv_Ratio)
                !Call SetSound( TDS%SoundRPM )
                TDS%Torque          = 0.0d0
                TopDrivePanel%TopDriveTorqueGauge = 0.d0           ![ft.lbf]
                TDS_OldPowerMode    = -1
            else if ( Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then
                if ( TopDrivePanel%RpmKnob>0. ) then
                    TopDrivePanel%TopDriveOperationFaultLed = 1
                    TDS%Speed = 0.d0                 !Speed [RPM]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = 0.d0      ![ft.lbf]
                    TDS_OldPowerMode     = -1
                else
                    TopDrivePanel%TopDriveOperationFaultLed = 0
                    TDS%Speed = 0.d0                 !Speed [RPM]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = 0.d0      ![ft.lbf]
                    TDS_OldPowerMode     = -1
                end if
            end if
        else
            TopDrivePanel%TopDriveOperationFaultLed = 0
            TDS%Speed = 0.d0                 !Speed [RPM]
            TopDrivePanel%TopDriveRpmGauge  = TDS%Speed
            TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
            TDS%SoundRPM      = 0
            !Call SetSound( TDS%SoundRPM )
            TDS%Torque           = 0.d0      ![ft.lbf]
            TopDrivePanel%TopDriveTorqueGauge  = 0.d0      ![ft.lbf]
            TDS_OldPowerMode     = -1
        end if
                
    else if ( TopDrivePanel%TopDriveTdsPowerState==1 .or. TDS_OldPowerMode==1 ) then  !REV
     
        if ( TopDrivePanel%TopDriveDrillTorqueState==0 .and. Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then  ! TdsMu_SPINE
            TopDrivePanel%TopDriveOperationFaultLed = 0
            TDS%Speed         = 0.d0              !Speed [RPM]
            TopDrivePanel%TopDriveRpmGauge  = 30.d0
            TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
            TDS%SoundRPM = 30
            !Call SetSound( TDS%SoundRPM )
            TDS%Torque          = 0.d0            ![ft.lbf]
            TopDrivePanel%TopDriveTorqueGauge = 0.d0
            TDS_OldPowerMode    = 1
        else if ( TopDrivePanel%TopDriveDrillTorqueState==0 .and. Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then  ! TdsMu_SPINE
            TopDrivePanel%TopDriveOperationFaultLed = 0
            TDS%Speed         = 0.d0              !Speed [RPM]
            TopDrivePanel%TopDriveRpmGauge  = 30.d0
            TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
            TDS%SoundRPM = 0
            !Call SetSound( TDS%SoundRPM )
            TDS%Torque          = 0.d0            ![ft.lbf]
            TopDrivePanel%TopDriveTorqueGauge = 1000.d0
            TDS_OldPowerMode    = 1
        else if ( TopDrivePanel%TopDriveDrillTorqueState==1 ) then  ! TdsMu_Torque
            if ( TopDrivePanel%RpmKnob>0. ) then
                TopDrivePanel%TopDriveOperationFaultLed = 1
                TDS%Speed = 0.d0                  !Speed [RPM]
                TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
                TDS%SoundRPM = 0
                !Call SetSound( TDS%SoundRPM )
                TDS%Torque           = 0.d0       ![ft.lbf]
                TopDrivePanel%TopDriveTorqueGauge  = 0.d0       ![ft.lbf]
                TDS_OldPowerMode     = 1
            else
                TopDrivePanel%TopDriveOperationFaultLed = 0
                if ( Get_TdsConnectionModes()==TDS_CONNECTION_STRING ) then
                    TDS%Speed = 0.d0                 !Speed [RPM]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = 2000.d0
                    TDS_OldPowerMode     = 1
                else if ( Get_TdsConnectionModes()==Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then
                    TDS%Speed = 0.d0                 !Speed [RPM]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = 0.d0      ![ft.lbf]
                    TDS_OldPowerMode     = 1
                else if ( Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then
                    TDS%Speed = 0.d0                 !Speed [RPM]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = 1000.d0   ![ft.lbf]
                    TDS_OldPowerMode     = -1
                end if
            end if
        else if ( TopDrivePanel%TopDriveDrillTorqueState==-1 ) then  ! TdsMu_DRILL
            if ( Get_TdsConnectionModes()==TDS_CONNECTION_STRING ) then
                TopDrivePanel%TopDriveOperationFaultLed = 0
                TDS%Speed         = (30.d0*TDS%w_new/pi)/TDS%Conv_Ratio     !Speed [RPM]
                TopDrivePanel%TopDriveRpmGauge  = TDS%Speed
                TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
                TDS%SoundRPM      = INT(TDS%Speed)
                !Call SetSound( TDS%SoundRPM )
                TDS%Torque         = ( ((TDS%J_coef+TDS%String_JCoef)*(((TDS%w_new/TDS%Conv_Ratio)-TDS_wOld)/TDS%time_step))+(TDS%String_Torque) )*0.73756215   ![N.m]*0.73756215 = [ft.lbf]
                TopDrivePanel%TopDriveTorqueGauge= TDS%Torque
                TDS_OldPowerMode   = 1
            else if ( Get_TdsConnectionModes()==Get_TdsConnectionModes()==TDS_CONNECTION_NOTHING ) then
                TopDrivePanel%TopDriveOperationFaultLed = 0
                TDS%Speed = 0.0d0
                TopDrivePanel%TopDriveRpmGauge  = ((30.d0*TDS%w_new/pi)/TDS%Conv_Ratio)    !Speed [RPM]
                TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
                TDS%SoundRPM      = INT((30.d0*TDS%w_new/pi)/TDS%Conv_Ratio)
                !Call SetSound( TDS%SoundRPM )
                TDS%Torque          = 0.0d0
                TopDrivePanel%TopDriveTorqueGauge = 0.d0           ![ft.lbf]
                TDS_OldPowerMode    = 1
            else if ( Get_TdsConnectionModes()==TDS_CONNECTION_SPINE ) then
                if ( TopDrivePanel%RpmKnob>0. ) then
                    TopDrivePanel%TopDriveOperationFaultLed = 1
                    TDS%Speed = 0.d0                 !Speed [RPM]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = 0.d0      ![ft.lbf]
                    TDS_OldPowerMode     = 1
                else
                    TopDrivePanel%TopDriveOperationFaultLed = 0
                    TDS%Speed = 0.d0                 !Speed [RPM]
                    TopDrivePanel%TopDriveRpmGauge = TDS%Speed
                    TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
                    TDS%SoundRPM = 0
                    !Call SetSound( TDS%SoundRPM )
                    TDS%Torque           = 0.d0      ![ft.lbf]
                    TopDrivePanel%TopDriveTorqueGauge  = 0.d0      ![ft.lbf]
                    TDS_OldPowerMode     = 1
                end if
            end if
        else
            TopDrivePanel%TopDriveOperationFaultLed = 0
            TDS%Speed = 0.d0                 !Speed [RPM]
            TopDrivePanel%TopDriveRpmGauge  = TDS%Speed
            TD_RPMUnityOutput = -TopDrivePanel%TopDriveRpmGauge
            TDS%SoundRPM      = 0
            !Call SetSound( TDS%SoundRPM )
            TDS%Torque           = 0.d0      ![ft.lbf]
            TopDrivePanel%TopDriveTorqueGauge  = 0.d0      ![ft.lbf]
            TDS_OldPowerMode     = 1
        end if
        
    end if
    
    
    
    
    
    
    
!******************************************************************
    
    
    !!!!TDS%Output_Current = (TDS%TracTorque*TDS%w_new)/TDS%Vt     !????????????
    !print*, 'Power_sigma=', Power_sigma
    !print*, 'power_num_of_Jenerators=', power_num_of_Jenerators
    !print*, 'drilling_num_of_Jenerators=', drilling_num_of_Jenerators
    !!print*, 'Jenerator_power=', Jenerator_power
    !print*, 'max_Power_sigma=', max_Power_sigma
    !print*, 'TDS%Vt=', TDS%Vt
    !!print*, 'TDS%w=', TDS%w_new
    !!!print*, 'TDS%String_Torque=', TDS%String_Torque
    !!print*, 'TDS%Speed=', TDS%Speed
    !!print*, 'TDS%Speed2=', ((30.*TDS%w_new/pi)/TDS%Conv_Ratio)
    !!!!!print*, 'TDS%Te=', TDS%Te
    !!print*, 'TDS%TL=', TDS%TL
    !print*, 'TDS%ia=', TDS%ia_new
    !print*, 'TDS%ia_ref=', TDS%ia_ref
    !print*, 'TDS%ia_ref_limit=', TDS%ia_ref_limit
    
    
END subroutine