SimulationCore2/Equipments/TopDrive/TopDrive_Traction_Motor.f90

subroutine TopDrive_Traction_Motor

    use CDrillingConsoleVariables
    use CDataDisplayConsoleVariables
    ! use CSimulationVariables
    use CTopDrivePanelVariables
    use TopDrive_VARIABLES
    use equipments_PowerLimit
 
    IMPLICIT NONE
    
    !integer :: jnomb
    !jnomb = 0
    
    !>>>>>>>>>>>>>>>>>>>>>>>  DATA  <<<<<<<<<<<<<<<<<<<<<<<<<<<    
    TDS%TL = (TDS%String_Torque+TDS%ConstLoad)/(TDS%Mech_Efficiency*TDS%Conv_Ratio)
    
    La = 1700.*1d-6
    Lf = 260.*1d-6
    Ra = 9.5*1d-3
    !Rf = 5.4*1d-3
    Rf = 0.
    
    !******** controller *******
    Kpn = 50.d0
    Kin = 3.d0
    Kpi = 100.d0
    Kii = 900.d0
    
    TDS%time  = TDS%time_step
    TDS%dt    = 1.d-5
    TDS%error = .001
    
!>>>>>>>>>>>>>>>>>>>>>>>>>>>><<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    
    TDS%n            = TDS%time/TDS%dt
    TDS%w_ref        = (pi*(TDS%N_ref)/30.)
    TDS%ia_ref_limit = TopDrivePanel%TopDriveTorqueLimitKnob/60.d0
    
    TDS%ia_er = 1.
    TDS%w_er  = 1.
    TDS%x_er  = 1.
    TDS%y_er  = 1.
    
    TDS%i = 1
    
!>>>>>>>>>>>>>>>>>>>>>>>>>>>><<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    DO WHILE (TDS%i<=TDS%n)
  
        
        !>>>>>>>>>>>>  Runge-Kutta Method (4th order)  <<<<<<<<<<<<<<
          
        call TDS_dx((TDS%i*TDS%dt),TDS%ia,TDS%w,TDS%x,TDS%y)
        call TDS_dy((TDS%i*TDS%dt),TDS%ia,TDS%w,TDS%x,TDS%y)
        call TDS_dia((TDS%i*TDS%dt),TDS%ia,TDS%w,TDS%fii,TDS%x,TDS%y)
        call TDS_dw((TDS%i*TDS%dt),TDS%ia,TDS%w,TDS%fii,TDS%TL)
        TDS%K1x  = TDS%dt*TDS%dx
        TDS%K1y  = TDS%dt*TDS%dy
        TDS%K1ia = TDS%dt*TDS%dia
        TDS%K1w  = TDS%dt*TDS%dw
          
        call TDS_dx((TDS%i*TDS%dt)+(TDS%dt/2.),TDS%ia+(TDS%K1ia/2.),TDS%w+(TDS%K1w/2.),TDS%x+(TDS%K1x/2.),TDS%y+(TDS%K1y/2.))
        call TDS_dy((TDS%i*TDS%dt)+(TDS%dt/2.),TDS%ia+(TDS%K1ia/2.),TDS%w+(TDS%K1w/2.),TDS%x+(TDS%K1x/2.),TDS%y+(TDS%K1y/2.))
        call TDS_dia((TDS%i*TDS%dt)+(TDS%dt/2.),TDS%ia+(TDS%K1ia/2.),TDS%w+(TDS%K1w/2.),TDS%fii,TDS%x+(TDS%K1x/2.),TDS%y+(TDS%K1y/2.))
        call TDS_dw((TDS%i*TDS%dt)+(TDS%dt/2.),TDS%ia+(TDS%K1ia/2.),TDS%w+(TDS%K1w/2.),TDS%fii,TDS%TL)
        TDS%K2x  = TDS%dt*TDS%dx
        TDS%K2y  = TDS%dt*TDS%dy
        TDS%K2ia = TDS%dt*TDS%dia
        TDS%K2w  = TDS%dt*TDS%dw
         
        call TDS_dx((TDS%i*TDS%dt)+(TDS%dt/2.),TDS%ia+(TDS%K2ia/2.),TDS%w+(TDS%K2w/2.),TDS%x+(TDS%K2x/2.),TDS%y+(TDS%K2y/2.))
        call TDS_dy((TDS%i*TDS%dt)+(TDS%dt/2.),TDS%ia+(TDS%K2ia/2.),TDS%w+(TDS%K2w/2.),TDS%x+(TDS%K2x/2.),TDS%y+(TDS%K2y/2.))
        call TDS_dia((TDS%i*TDS%dt)+(TDS%dt/2.),TDS%ia+(TDS%K2ia/2.),TDS%w+(TDS%K2w/2.),TDS%fii,TDS%x+(TDS%K2x/2.),TDS%y+(TDS%K2y/2.))
        call TDS_dw((TDS%i*TDS%dt)+(TDS%dt/2.),TDS%ia+(TDS%K2ia/2.),TDS%w+(TDS%K2w/2.),TDS%fii,TDS%TL)
        TDS%K3x  = TDS%dt*TDS%dx
        TDS%K3y  = TDS%dt*TDS%dy
        TDS%K3ia = TDS%dt*TDS%dia
        TDS%K3w  = TDS%dt*TDS%dw
         
        call TDS_dx((TDS%i*TDS%dt)+TDS%dt,TDS%ia+TDS%K3ia,TDS%w+TDS%K3w,TDS%x+TDS%K3x,TDS%y+TDS%K3y)
        call TDS_dy((TDS%i*TDS%dt)+TDS%dt,TDS%ia+TDS%K3ia,TDS%w+TDS%K3w,TDS%x+TDS%K3x,TDS%y+TDS%K3y)
        call TDS_dia((TDS%i*TDS%dt)+TDS%dt,TDS%ia+TDS%K3ia,TDS%w+TDS%K3w,TDS%fii,TDS%x+TDS%K3x,TDS%y+TDS%K3y)
        call TDS_dw((TDS%i*TDS%dt)+TDS%dt,TDS%ia+TDS%K3ia,TDS%w+TDS%K3w,TDS%fii,TDS%TL)
        TDS%K4x  = TDS%dt*TDS%dx
        TDS%K4y  = TDS%dt*TDS%dy
        TDS%K4ia = TDS%dt*TDS%dia
        TDS%K4w  = TDS%dt*TDS%dw
        
        TDS%x_new  = TDS%x_old+((TDS%K1x+(2.*TDS%K2x)+(2.*TDS%K3x)+TDS%K4x)/6.)
        TDS%y_new  = TDS%y_old+((TDS%K1y+(2.*TDS%K2y)+(2.*TDS%K3y)+TDS%K4y)/6.)
        TDS%ia_new = TDS%ia_old+((TDS%K1ia+(2.*TDS%K2ia)+(2.*TDS%K3ia)+TDS%K4ia)/6.)
        TDS%w_new  = TDS%w_old+((TDS%K1w+(2.*TDS%K2w)+(2.*TDS%K3w)+TDS%K4w)/6.)
          
        !>>>>>>>>>>>>>>>>>>>>>>>>>>>><<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

        TDS%x_old  = TDS%x_new
        TDS%y_old  = TDS%y_new
        TDS%ia_old = TDS%ia_new
        TDS%w_old  = TDS%w_new
        TDS%x      = TDS%x_new
        TDS%y      = TDS%y_new
        TDS%ia     = TDS%ia_new
        TDS%w      = TDS%w_new
        TDS%Te     = TDS%fii*TDS%ia_new
        
        !if (jnomb==0) then
        !    print*, 'TDS%Vt0=', TDS%Vt
        !    jnomb=1
        !end if
        TDS%ia_ref = TDS%y_new+Kpn*((30.0*TDS%w_ref/pi)-(30.0*TDS%w_new/pi))
        call RTTorqueLimit
        TDS%Vt     = TDS%x_new+(Kpi*(TDS%ia_ref-TDS%ia_new))
        !call PowerLimits
        !if (Power_sigma>max_Power_sigma) then
        !    TDS%Vt = TDS%Vt
        !else
        !    TDS%Vt = TDS%x_new+(Kpi*(TDS%ia_ref-TDS%ia_new))
        !end if
          
        IF (TDS%Vt>810.) THEN
            TDS%Vt = 810.0
        ELSE IF (TDS%Vt<0.) THEN
            TDS%Vt = 0.0
        END IF
        
              
            
              
        TDS%i = TDS%i+1
        
        
    END DO
!>>>>>>>>>>>>>>>>>>>>>>>>>>>><<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    
   
    
    
    
END subroutine