def DISCON(avrSWAP_py, from_SC_py, to_SC_py): print("SIAMO ENTRATI IN DISCON.py") print("from_SC_py in DISCON.py: ", from_SC_py) VS_RtGnSp = 121.6805 VS_SlPc = 10.00 VS_Rgn2K = 2.332287 VS_Rgn2Sp = 91.21091 VS_CtInSp = 70.16224 VS_RtPwr = 5296610.0 CornerFreq = 1.570796 #1.570796 PC_MaxPit = 0.2875 # ERA 1.570796 rad PC_DT = 0.000125 VS_DT = 0.000125 OnePlusEps = 1 + sys.float_info.epsilon VS_MaxTq = 47402.91 BlPitch = numpy.zeros(3) PitRate = numpy.zeros(3) VS_Rgn3MP = 0.01745329 PC_KK = 0.1099965 PC_KI = 0.008068634 PC_KP = 0.01882681 PC_RefSpd = 122.9096 VS_MaxRat = 15000.0 PC_MaxRat = 0.1396263 #0.1396263 iStatus = int(round(avrSWAP_py[0])) NumBl = int(round(avrSWAP_py[60])) PC_MinPit = 0.0 PC_MinPit1 = from_SC_py print("PC_MinPit in DISCON.py: ", PC_MinPit) print("NumBl in DISCON.py: ", NumBl) print("OnePLUSEps ", OnePlusEps) BlPitch[0] = min( max( avrSWAP_py[3], PC_MinPit ), PC_MaxPit ) BlPitch[1] = min( max( avrSWAP_py[32], PC_MinPit ), PC_MaxPit ) BlPitch[2] = min( max( avrSWAP_py[33], PC_MinPit ), PC_MaxPit ) GenSpeed = avrSWAP_py[19] HorWindV = avrSWAP_py[26] Time = avrSWAP_py[1] aviFAIL_py = 0 if iStatus == 0: globalDISCON.VS_SySp = VS_RtGnSp/( 1.0 + 0.01*VS_SlPc ) globalDISCON.VS_Slope15 = ( VS_Rgn2K*VS_Rgn2Sp*VS_Rgn2Sp )/( VS_Rgn2Sp - VS_CtInSp ) globalDISCON.VS_Slope25 = ( VS_RtPwr/VS_RtGnSp )/( VS_RtGnSp - globalDISCON.VS_SySp ) if VS_Rgn2K == 0: globalDISCON.VS_TrGnSp = globalDISCON.VS_SySp else: globalDISCON.VS_TrGnSp = ( globalDISCON.VS_Slope25 - math.sqrt(globalDISCON.VS_Slope25*( globalDISCON.VS_Slope25 - 4.0*VS_Rgn2K*globalDISCON.VS_SySp ) ) )/( 2.0*VS_Rgn2K ) globalDISCON.GenSpeedF = GenSpeed globalDISCON.PitCom = BlPitch print("PitCom: ", globalDISCON.PitCom) print("BlPitch: ", BlPitch) GK = 1.0/( 1.0 + globalDISCON.PitCom[0]/PC_KK ) globalDISCON.IntSpdErr = globalDISCON.PitCom[0]/( GK*PC_KI ) globalDISCON.LastTime = Time globalDISCON.LastTimePC = Time - PC_DT globalDISCON.LastTimeVS = Time - VS_DT print("0") if iStatus >= 0 and aviFAIL_py >= 0: avrSWAP_py[35] = 0.0 avrSWAP_py[40] = 0.0 avrSWAP_py[45] = 0.0 avrSWAP_py[47] = 0.0 avrSWAP_py[64] = 0.0 avrSWAP_py[71] = 0.0 avrSWAP_py[78] = 0.0 avrSWAP_py[79] = 0.0 avrSWAP_py[80] = 0.0 Alpha = math.exp( ( globalDISCON.LastTime - Time )*CornerFreq ) globalDISCON.GenSpeedF = ( 1.0 - Alpha )*GenSpeed + Alpha*globalDISCON.GenSpeedF ElapTime = Time - globalDISCON.LastTimeVS print("1 ", ElapTime) print("globalDISCON.LastTimeVS: ", globalDISCON.LastTimeVS) print("Time*OnePlusEps - globalDISCON.LastTimeVS: ", Time*OnePlusEps - globalDISCON.LastTimeVS) if ( Time*OnePlusEps - globalDISCON.LastTimeVS ) >= VS_DT: print("GenSPeedF: ", globalDISCON.GenSpeedF) print("PitCom: ", globalDISCON.PitCom[0]) if globalDISCON.GenSpeedF >= VS_RtGnSp or globalDISCON.PitCom[0] >= VS_Rgn3MP: GenTrq = VS_RtPwr/globalDISCON.GenSpeedF print("A") print("GenTrq: ", GenTrq) elif globalDISCON.GenSpeedF <= VS_CtInSp: GenTrq = 0.0 print("B") elif globalDISCON.GenSpeedF < VS_Rgn2Sp: GenTrq = globalDISCON.VS_Slope15*( globalDISCON.GenSpeedF - VS_CtInSp ) print("C") elif globalDISCON.GenSpeedF < globalDISCON.VS_TrGnSp: GenTrq = VS_Rgn2K*globalDISCON.GenSpeedF*globalDISCON.GenSpeedF print("D") else: GenTrq = globalDISCON.VS_Slope25*( globalDISCON.GenSpeedF - globalDISCON.VS_SySp ) print("E") GenTrq = min(GenTrq, VS_MaxTq) print("2: ", GenTrq) if iStatus == 0: globalDISCON.LastGenTrq = GenTrq TrqRate = ( GenTrq - globalDISCON.LastGenTrq )/ElapTime TrqRate = min( max( TrqRate, -VS_MaxRat ), VS_MaxRat ) GenTrq = globalDISCON.LastGenTrq + TrqRate*ElapTime globalDISCON.LastTimeVS = Time globalDISCON.LastGenTrq = GenTrq print("3") avrSWAP_py[34] = 1.0 avrSWAP_py[55] = 0.0 avrSWAP_py[46] = globalDISCON.LastGenTrq print("Time ", Time) ElapTime = Time - globalDISCON.LastTimePC print("ELAP Time ", ElapTime) print("LASTTIMEPC Time ", globalDISCON.LastTimePC) if ( Time*OnePlusEps - globalDISCON.LastTimePC ) >= PC_DT: GK = 1.0/( 1.0 + globalDISCON.PitCom[0]/PC_KK ) SpdErr = globalDISCON.GenSpeedF - PC_RefSpd globalDISCON.IntSpdErr = globalDISCON.IntSpdErr + SpdErr*ElapTime globalDISCON.IntSpdErr = min( max( globalDISCON.IntSpdErr, PC_MinPit/( GK*PC_KI ) ), PC_MaxPit/( GK*PC_KI ) ) PitComP = GK*PC_KP* SpdErr PitComI = GK*PC_KI*globalDISCON.IntSpdErr PitComT = PitComP + PitComI PitComT = min( max( PitComT, PC_MinPit ), PC_MaxPit ) for i in range(NumBl): PitRate[i] = ( PitComT - BlPitch[i] )/ElapTime PitRate[i] = min( max( PitRate[i], -PC_MaxRat ), PC_MaxRat ) globalDISCON.PitCom[i] = BlPitch[i] + PitRate[i]*ElapTime globalDISCON.PitCom[i] = min( max( globalDISCON.PitCom[i], PC_MinPit ), PC_MaxPit ) globalDISCON.LastTimePC = Time print("4") print("PitCom: ", globalDISCON.PitCom) avrSWAP_py[54] = 0.0 avrSWAP_py[41] = globalDISCON.PitCom[0] avrSWAP_py[42] = globalDISCON.PitCom[1] avrSWAP_py[43] = globalDISCON.PitCom[2] avrSWAP_py[44] = globalDISCON.PitCom[0] if 'GenTrq' in locals(): to_SC_py = GenTrq else: to_SC_py = globalDISCON.LastGenTrq globalDISCON.LastTime = Time print("globalDISCON.LastTime: ", globalDISCON.LastTime) avrSWAP_py = numpy.append(avrSWAP_py,to_SC_py) print("to_SC_py in DISCON.py: ", to_SC_py) file = open("Bl1outin.txt","a+") file.write("%f, %f, %f \n" % (avrSWAP_py[29], avrSWAP_py[68], Time)) file.close() file = open("Bl2outin.txt","a+") file.write("%f, %f, %f \n" % (avrSWAP_py[30], avrSWAP_py[69], Time)) file.close() file = open("Bl3outin.txt","a+") file.write("%f, %f, %f \n" % (avrSWAP_py[31], avrSWAP_py[70], Time)) file.close() file = open("Azimuth.txt","a+") file.write("%f, %f, %f, %f \n" % (avrSWAP_py[59], avrSWAP_py[20], avrSWAP_py[26], Time)) file.close() tmp = float(OBSERVER.tmp) #POSG acc = float(OBSERVER.acc) #POSR OBSERVER.y = avrSWAP_py[19] print("tmp: ", OBSERVER.tmp) print("acc: ", OBSERVER.acc) print("y: ", OBSERVER.y) OBSERVER.Qg = avrSWAP_py[22] print("Qg: ", avrSWAP_py[22]) if numpy.isclose(Time, 0.0): x0 = numpy.array([1.5, 120, 0, 0]) xsol1 = numpy.array([1.5, 120, 0, 0]) OBSERVER.xsol = xsol1 xsol = numpy.zeros((10,4)) xsol[9,:] = xsol1 xppsolin = numpy.array([[0, 0, 0, 0], [0, 0, 1.5, 120]]) print(xsol) else: x0 = OBSERVER.xsol ts = numpy.linspace(Time, Time + 0.005, 10) xsol = odeint(OBSERVER.dx_dt, x0, ts, args=(float(OBSERVER.y), float(OBSERVER.tmp))) print("SOL SHAPE: ", numpy.shape(xsol)) OBSERVER.xsol = xsol[9,:] OBSERVER.xsolold = numpy.vstack((OBSERVER.xsolold, OBSERVER.xsol)) xppsolin = numpy.gradient(OBSERVER.xsolold, 0.005, axis=0) print("SOL: ", xsol) print("XOLD: ", OBSERVER.xsolold) xppsol = OBSERVER.xpp(xsol[9,:], float(OBSERVER.y), float(OBSERVER.tmp)) print("INERTIA: ", xppsol) print("INERTIA: ", xppsolin[-1,:]) Qasol = OBSERVER.Qacalc(xppsolin[-1,:], xsol[9,:], float(OBSERVER.y), float(OBSERVER.tmp)) OBSERVER.tmp = float(avrSWAP_py[19]*0.005 + tmp) OBSERVER.acc = float(avrSWAP_py[20]*0.005 + acc) error = (Qasol - (avrSWAP_py[13]/avrSWAP_py[20]))/(avrSWAP_py[13]/avrSWAP_py[20]) errorposg = (OBSERVER.tmp-xsol[9,3])/xsol[9,3] errorposr = (OBSERVER.acc-xsol[9,2])/xsol[9,2] errorwr = (avrSWAP_py[20]-xsol[9,0])/avrSWAP_py[20] errorwg = (avrSWAP_py[19]-xsol[9,1])/avrSWAP_py[19] pitch_obs = (avrSWAP_py[3]+avrSWAP_py[32]+avrSWAP_py[33])*180/(3*numpy.pi) if pitch_obs > 15: pitch_obs = 15 elif pitch_obs < -10: pitch_obs = -10 num = (2*Qasol)/(numpy.pi*OBSERVER.rho*(xsol[9,0]**2)*(OBSERVER.R**5)) tsr_obs = optim.fsolve(OBSERVER.func_impl, 4.5, args=(num, pitch_obs)) vento_obs = xsol[9,0]*OBSERVER.R/tsr_obs if vento_obs > 20: vento_obs = 20 elif vento_obs < 4: vento_obs = 4 file = open("Error.txt","a+") file.write("%f, %f \n" % (error, Time)) file.close() file = open("ErrorPosg.txt","a+") file.write("%f, %f \n" % (errorposg, Time)) file.close() file = open("ErrorPosr.txt","a+") file.write("%f, %f \n" % (errorposr, Time)) file.close() file = open("ErrorWG.txt","a+") file.write("%f, %f \n" % (errorwg, Time)) file.close() file = open("ErrorWR.txt","a+") file.write("%f, %f \n" % (errorwr, Time)) file.close() file = open("EWR.txt","a+") file.write("%f, %f \n" % (avrSWAP_py[20], Time)) file.close() file = open("EWG.txt","a+") file.write("%f, %f \n" % (avrSWAP_py[19], Time)) file.close() file = open("EXSOL.txt","a+") file.write("%f, %f, %f, %f, %f \n" % (xsol[9,0], xsol[9,1], xsol[9,2], xsol[9,3], Time)) file.close() file = open("EPOSG.txt","a+") file.write("%f, %f \n" % (tmp, Time)) file.close() file = open("EPOSR.txt","a+") file.write("%f, %f \n" % (acc, Time)) file.close() file = open("EACC.txt","a+") file.write("%f, %f, %f, %f, %f \n" % (xppsolin[-1,0], xppsolin[-1,1], xppsolin[-1,2], xppsolin[-1,3], Time)) file.close() file = open("EPitch.txt","a+") file.write("%f, %f, %f \n" % ((avrSWAP_py[3]+avrSWAP_py[32]+avrSWAP_py[33])*180/(3*numpy.pi), pitch_obs, Time)) file.close() file = open("EWIND.txt","a+") file.write("%f, %f \n" % (vento_obs, Time)) file.close() file = open("EQasol.txt","a+") file.write("%f, %f \n" % (Qasol, Time)) file.close() file = open("ENum.txt","a+") file.write("%f, %f \n" % (num, Time)) file.close() print("ERROR: ", error) print("Qa: ", Qasol) print("Qareal: ", avrSWAP_py[13]/avrSWAP_py[20]) print("POWER: ", avrSWAP_py[13]) #WIND YAW ERROR OBSERVER SECTION blmom1 = numpy.array([avrSWAP_py[29], avrSWAP_py[68]]) blmom2 = numpy.array([avrSWAP_py[30], avrSWAP_py[69]]) blmom3 = numpy.array([avrSWAP_py[31], avrSWAP_py[70]]) #azimuth = numpy.array([avrSWAP_py[59], avrSWAP_py[59] + 2*numpy.pi/3, avrSWAP_py[59] + 4*numpy.pi/3]) #wryaw = avrSWAP_py[20] #USING DATA FROM WIND OBSERVER!!! FILTRO VELOCITA E VENTO MISURATO #azimuth = numpy.array([xsol[9,2], xsol[9,2] + 2*numpy.pi/3, xsol[9,2] + 4*numpy.pi/3]) azimuth = numpy.array([(xsol[9,2] % (numpy.pi*2)), ((xsol[9,2] + 2*numpy.pi/3) % (numpy.pi*2)), ((xsol[9,2] + 4*numpy.pi/3) % (numpy.pi*2))]) #azimuth = numpy.array([(xsol[9,2] % (numpy.pi*2)), ((xsol[9,2] % (numpy.pi*2)) + 2*numpy.pi/3), ((xsol[9,2] % (numpy.pi*2)) + 4*numpy.pi/3)]) #yawerrmeas.wryaw[-2] = yawerrmeas.wryaw[-1] #yawerrmeas.wryaw[-1] = xsol[9,0] #yawerrmeas.wryaw_f[-2] = yawerrmeas.wryaw_f[-1] #yawerrmeas.wryaw_f[-1] = (-(yawerrmeas.dtFAST - yawerrmeas.tau_mis)*yawerrmeas.wryaw_f[-2] + yawerrmeas.dtFAST*yawerrmeas.wryaw[-1] + yawerrmeas.dtFAST*yawerrmeas.wryaw[-2])/(yawerrmeas.dtFAST + yawerrmeas.tau_mis) wryaw = xsol[9,0] tau = yawerrmeas.tau_nn*(2*numpy.pi) / wryaw #ANCHE QUI 3P ??? Periodo o frequenza??? print(tau, blmom1, blmom2, blmom3, azimuth, wryaw) ColT = yawerrmeas.ColTransf(azimuth[0], azimuth[1], azimuth[2]) m_out = numpy.array([blmom1[0], blmom2[0], blmom3[0]]) m_in = numpy.array([blmom1[1], blmom2[1], blmom3[1]]) m_out_tr = numpy.dot(ColT, m_out.transpose()) m_in_tr = numpy.dot(ColT, m_in.transpose()) print(m_out_tr) yawerrmeas.blmom1_tr[-2,:] = yawerrmeas.blmom1_tr[-1,:] yawerrmeas.blmom2_tr[-2,:] = yawerrmeas.blmom2_tr[-1,:] yawerrmeas.blmom3_tr[-2,:] = yawerrmeas.blmom3_tr[-1,:] yawerrmeas.blmom1_tr[-1,:] = numpy.array([m_out_tr[0], m_in_tr[0]]) yawerrmeas.blmom2_tr[-1,:] = numpy.array([m_out_tr[1], m_in_tr[1]]) yawerrmeas.blmom3_tr[-1,:] = numpy.array([m_out_tr[2], m_in_tr[2]]) print(yawerrmeas.blmom1_tr) #if Time >= 10.0: m_out1 = (-(yawerrmeas.dtFAST - tau)*yawerrmeas.m_out1[-1] + yawerrmeas.dtFAST*yawerrmeas.blmom1_tr[-1,0] + yawerrmeas.dtFAST*yawerrmeas.blmom1_tr[-2,0])/(yawerrmeas.dtFAST + tau) m_in1 = (-(yawerrmeas.dtFAST - tau)*yawerrmeas.m_in1[-1] + yawerrmeas.dtFAST*yawerrmeas.blmom1_tr[-1,1] + yawerrmeas.dtFAST*yawerrmeas.blmom1_tr[-2,1])/(yawerrmeas.dtFAST + tau) m_out2 = (-(yawerrmeas.dtFAST - tau)*yawerrmeas.m_out2[-1] + yawerrmeas.dtFAST*yawerrmeas.blmom2_tr[-1,0] + yawerrmeas.dtFAST*yawerrmeas.blmom2_tr[-2,0])/(yawerrmeas.dtFAST + tau) m_in2 = (-(yawerrmeas.dtFAST - tau)*yawerrmeas.m_in2[-1] + yawerrmeas.dtFAST*yawerrmeas.blmom2_tr[-1,1] + yawerrmeas.dtFAST*yawerrmeas.blmom2_tr[-2,1])/(yawerrmeas.dtFAST + tau) m_out3 = (-(yawerrmeas.dtFAST - tau)*yawerrmeas.m_out3[-1] + yawerrmeas.dtFAST*yawerrmeas.blmom3_tr[-1,0] + yawerrmeas.dtFAST*yawerrmeas.blmom3_tr[-2,0])/(yawerrmeas.dtFAST + tau) m_in3 = (-(yawerrmeas.dtFAST - tau)*yawerrmeas.m_in3[-1] + yawerrmeas.dtFAST*yawerrmeas.blmom3_tr[-1,1] + yawerrmeas.dtFAST*yawerrmeas.blmom3_tr[-2,1])/(yawerrmeas.dtFAST + tau) yawerrmeas.m_out1[-2] = yawerrmeas.m_out1[-1] yawerrmeas.m_in1[-2] = yawerrmeas.m_in1[-1] yawerrmeas.m_out2[-2] = yawerrmeas.m_out2[-1] yawerrmeas.m_in2[-2] = yawerrmeas.m_in2[-1] yawerrmeas.m_out3[-2] = yawerrmeas.m_out3[-1] yawerrmeas.m_in3[-2] = yawerrmeas.m_in3[-1] yawerrmeas.m_out1[-1] = m_out1 yawerrmeas.m_in1[-1] = m_in1 yawerrmeas.m_out2[-1] = m_out2 yawerrmeas.m_in2[-1] = m_in2 yawerrmeas.m_out3[-1] = m_out3 yawerrmeas.m_in3[-1] = m_in3 '''else: m_out1 = m_out_tr[0] m_in1 = m_in_tr[0] m_out2 = m_out_tr[1] m_in2 = m_in_tr[1] m_out3 = m_out_tr[2] m_in3 = m_in_tr[2] yawerrmeas.m_out1[-2] = yawerrmeas.m_out1[-1] yawerrmeas.m_in1[-2] = yawerrmeas.m_in1[-1] yawerrmeas.m_out2[-2] = yawerrmeas.m_out2[-1] yawerrmeas.m_in2[-2] = yawerrmeas.m_in2[-1] yawerrmeas.m_out3[-2] = yawerrmeas.m_out3[-1] yawerrmeas.m_in3[-2] = yawerrmeas.m_in3[-1] yawerrmeas.m_out1[-1] = m_out1 yawerrmeas.m_in1[-1] = m_in1 yawerrmeas.m_out2[-1] = m_out2 yawerrmeas.m_in2[-1] = m_in2 yawerrmeas.m_out3[-1] = m_out3 yawerrmeas.m_in3[-1] = m_in3''' m_yaw_u0 = numpy.array([m_out2/m_out1, m_out3/m_out1, m_in2/m_in1, m_in3/m_in1]) m_yaw_k1 = numpy.array([1, m_out2, m_out3, m_in2, m_in3]) m_yaw = numpy.hstack((m_yaw_u0, m_yaw_k1)) Tmat = yawerrmeas.Tmat_int(vento_obs) #Tmat = yawerrmeas.Tmat_int(HorWindV) ris_yaw = numpy.dot(Tmat, m_yaw.transpose()) crosswind = wryaw*yawerrmeas.R*ris_yaw[0] vertshear = wryaw*yawerrmeas.R*ris_yaw[1]/vento_obs #vertshear = wryaw*yawerrmeas.R*ris_yaw[1]/HorWindV file = open("ECROSS.txt","a+") file.write("%f, %f, %f \n" % (crosswind, vertshear, Time)) file.close() file = open("EAzimuth.txt","a+") file.write("%f, %f, %f, %f \n" % (azimuth[0], azimuth[1], azimuth[2], Time)) file.close() file = open("EMOM.txt","a+") file.write("%f, %f, %f, %f, %f, %f, %f \n" % (m_out1, m_out2, m_out3, m_in1, m_in2, m_in3, Time)) file.close() return avrSWAP_py
def DISCON(avrSWAP_py, from_SC_py, to_SC_py): if logic.counter == 0: import globalDISCON import OBSERVER import yawerrmeas logic.counter = logic.counter + 1 elif logic.counter == 1: import globalDISCON1 as globalDISCON import OBSERVER1 as OBSERVER import yawerrmeas1 as yawerrmeas logic.counter = logic.counter + 1 elif logic.counter == 2: import globalDISCON2 as globalDISCON import OBSERVER2 as OBSERVER import yawerrmeas2 as yawerrmeas logic.counter = 0 #print("SIAMO ENTRATI IN DISCON.py") #print("from_SC_py in DISCON.py: ", from_SC_py) #print(avrSWAP_py[95], avrSWAP_py[26]) VS_RtGnSp = 121.6805 VS_SlPc = 10.00 VS_Rgn2K = 2.332287 VS_Rgn2Sp = 91.21091 VS_CtInSp = 70.16224 VS_RtPwr = 5296610.0 CornerFreq = 1.570796 #1.570796 PC_MaxPit = 1.570796 # ERA 1.570796 rad PC_DT = 0.000125 VS_DT = 0.000125 OnePlusEps = 1 + sys.float_info.epsilon VS_MaxTq = 47402.91 BlPitch = numpy.zeros(3) PitRate = numpy.zeros(3) VS_Rgn3MP = 0.01745329 PC_KK = 0.1099965 PC_KI = 0.008068634 PC_KP = 0.01882681 PC_RefSpd = 122.9096 VS_MaxRat = 15000.0 PC_MaxRat = 0.1396263 #0.1396263 YawSpr = 9.02832e9 YawDamp = 1.916e7 YawIn = 2.60789e6 kdYaw = 1e7 kpYaw = 5e7 kiYaw = 1e9 tauF = (1 / 3) * ((2 * numpy.pi) / 1.2671) Ts = 0.005 iStatus = int(round(avrSWAP_py[0])) NumBl = int(round(avrSWAP_py[60])) PC_MinPit = 0.0 #print("PC_MinPit in DISCON.py: ", PC_MinPit) #print("NumBl in DISCON.py: ", NumBl) #print("OnePLUSEps ", OnePlusEps) BlPitch[0] = min(max(avrSWAP_py[3], PC_MinPit), PC_MaxPit) BlPitch[1] = min(max(avrSWAP_py[32], PC_MinPit), PC_MaxPit) BlPitch[2] = min(max(avrSWAP_py[33], PC_MinPit), PC_MaxPit) GenSpeed = avrSWAP_py[19] HorWindV = avrSWAP_py[26] Time = avrSWAP_py[1] aviFAIL_py = 0 if iStatus == 0: globalDISCON.VS_SySp = VS_RtGnSp / (1.0 + 0.01 * VS_SlPc) globalDISCON.VS_Slope15 = (VS_Rgn2K * VS_Rgn2Sp * VS_Rgn2Sp) / (VS_Rgn2Sp - VS_CtInSp) globalDISCON.VS_Slope25 = (VS_RtPwr / VS_RtGnSp) / ( VS_RtGnSp - globalDISCON.VS_SySp) if VS_Rgn2K == 0: globalDISCON.VS_TrGnSp = globalDISCON.VS_SySp else: globalDISCON.VS_TrGnSp = (globalDISCON.VS_Slope25 - math.sqrt( globalDISCON.VS_Slope25 * (globalDISCON.VS_Slope25 - 4.0 * VS_Rgn2K * globalDISCON.VS_SySp))) / (2.0 * VS_Rgn2K) globalDISCON.GenSpeedF = GenSpeed globalDISCON.PitCom = BlPitch #print("PitCom: ", globalDISCON.PitCom) #print("BlPitch: ", BlPitch) GK = 1.0 / (1.0 + globalDISCON.PitCom[0] / PC_KK) globalDISCON.IntSpdErr = globalDISCON.PitCom[0] / (GK * PC_KI) globalDISCON.LastTime = Time globalDISCON.LastTimePC = Time - PC_DT globalDISCON.LastTimeVS = Time - VS_DT print("0") if iStatus >= 0 and aviFAIL_py >= 0: avrSWAP_py[35] = 0.0 avrSWAP_py[40] = 0.0 avrSWAP_py[45] = 0.0 avrSWAP_py[47] = 0.0 avrSWAP_py[64] = 0.0 avrSWAP_py[71] = 0.0 avrSWAP_py[78] = 0.0 avrSWAP_py[79] = 0.0 avrSWAP_py[80] = 0.0 Alpha = math.exp((globalDISCON.LastTime - Time) * CornerFreq) globalDISCON.GenSpeedF = ( 1.0 - Alpha) * GenSpeed + Alpha * globalDISCON.GenSpeedF ElapTime = Time - globalDISCON.LastTimeVS print("1 ", ElapTime) print("globalDISCON.LastTimeVS: ", globalDISCON.LastTimeVS) print("Time*OnePlusEps - globalDISCON.LastTimeVS: ", Time * OnePlusEps - globalDISCON.LastTimeVS) if (Time * OnePlusEps - globalDISCON.LastTimeVS) >= VS_DT: print("GenSPeedF: ", globalDISCON.GenSpeedF) print("PitCom: ", globalDISCON.PitCom[0]) if globalDISCON.GenSpeedF >= VS_RtGnSp or globalDISCON.PitCom[ 0] >= VS_Rgn3MP: GenTrq = VS_RtPwr / globalDISCON.GenSpeedF print("A") print("GenTrq: ", GenTrq) elif globalDISCON.GenSpeedF <= VS_CtInSp: GenTrq = 0.0 print("B") elif globalDISCON.GenSpeedF < VS_Rgn2Sp: GenTrq = globalDISCON.VS_Slope15 * (globalDISCON.GenSpeedF - VS_CtInSp) print("C") elif globalDISCON.GenSpeedF < globalDISCON.VS_TrGnSp: GenTrq = VS_Rgn2K * globalDISCON.GenSpeedF * globalDISCON.GenSpeedF print("D") else: GenTrq = globalDISCON.VS_Slope25 * (globalDISCON.GenSpeedF - globalDISCON.VS_SySp) print("E") GenTrq = min(GenTrq, VS_MaxTq) print("2: ", GenTrq) if iStatus == 0: globalDISCON.LastGenTrq = GenTrq TrqRate = (GenTrq - globalDISCON.LastGenTrq) / ElapTime TrqRate = min(max(TrqRate, -VS_MaxRat), VS_MaxRat) GenTrq = globalDISCON.LastGenTrq + TrqRate * ElapTime globalDISCON.LastTimeVS = Time globalDISCON.LastGenTrq = GenTrq print("3") avrSWAP_py[34] = 1.0 avrSWAP_py[55] = 0.0 avrSWAP_py[46] = globalDISCON.LastGenTrq print("Time ", Time) ElapTime = Time - globalDISCON.LastTimePC print("ELAP Time ", ElapTime) print("LASTTIMEPC Time ", globalDISCON.LastTimePC) if (Time * OnePlusEps - globalDISCON.LastTimePC) >= PC_DT: GK = 1.0 / (1.0 + globalDISCON.PitCom[0] / PC_KK) SpdErr = globalDISCON.GenSpeedF - PC_RefSpd globalDISCON.IntSpdErr = globalDISCON.IntSpdErr + SpdErr * ElapTime globalDISCON.IntSpdErr = min( max(globalDISCON.IntSpdErr, PC_MinPit / (GK * PC_KI)), PC_MaxPit / (GK * PC_KI)) PitComP = GK * PC_KP * SpdErr PitComI = GK * PC_KI * globalDISCON.IntSpdErr PitComT = PitComP + PitComI PitComT = min(max(PitComT, PC_MinPit), PC_MaxPit) for i in range(NumBl): PitRate[i] = (PitComT - BlPitch[i]) / ElapTime PitRate[i] = min(max(PitRate[i], -PC_MaxRat), PC_MaxRat) globalDISCON.PitCom[i] = BlPitch[i] + PitRate[i] * ElapTime globalDISCON.PitCom[i] = min( max(globalDISCON.PitCom[i], PC_MinPit), PC_MaxPit) globalDISCON.LastTimePC = Time print("4") #print("PitCom: ", globalDISCON.PitCom) avrSWAP_py[54] = 0.0 avrSWAP_py[41] = globalDISCON.PitCom[0] avrSWAP_py[42] = globalDISCON.PitCom[1] avrSWAP_py[43] = globalDISCON.PitCom[2] avrSWAP_py[44] = globalDISCON.PitCom[0] # COMMANDING YAW RATE globalDISCON.YawAngleGAold = globalDISCON.YawAngleGA globalDISCON.YawAngleGA = from_SC_py # END OF COMMANDED YAW RATE ON TURBINE 1 avrSWAP_py[ 28] = 1 # --> YAW CONTROL 0 = SPEED CONTROL, 1 = TORQUE CONTROL # SETTING POSITION TO BE REACHED AT 0.1 rad --> PI CONTROLLER ( I is INTEGRAL of 0.1rad in time) # avrSwap_py[23] --> YawRate Good for PID -- Derivative term if numpy.isclose(Time, 0.0): globalDISCON.flagyaw = True if globalDISCON.YawAngleGA != globalDISCON.YawAngleGAold: globalDISCON.flagyaw = False globalDISCON.YawAngleGAprev = globalDISCON.YawAngleGAold if not numpy.isclose( avrSWAP_py[36], globalDISCON.YawAngleGA) and globalDISCON.flagyaw == False: #if (not numpy.isclose(avrSWAP_py[36], globalDISCON.PosYawRef)) and (not numpy.isclose(avrSWAP_py[23], 0.0)) and globalDISCON.flag_yaw == False: #globalDISCON.IntYawRef = globalDISCON.IntYawRef + globalDISCON.PosYawRef * ElapTime #globalDISCON.IntYaw = globalDISCON.IntYaw + avrSWAP_py[36] * ElapTime #avrSWAP_py[47] = kpYaw * (globalDISCON.PosYawRef - avrSWAP_py[36]) + kiYaw * (globalDISCON.IntYawRef - globalDISCON.IntYaw) if abs(globalDISCON.PosYawRef - globalDISCON.YawAngleGA) > 0.0075: globalDISCON.VelYawRef = numpy.sign( globalDISCON.YawAngleGA - globalDISCON.YawAngleGAprev) * 0.0349066 / 2 globalDISCON.PosYawRef = globalDISCON.PosYawRef + globalDISCON.VelYawRef * ElapTime else: #globalDISCON.flagyaw = True globalDISCON.VelYawRef = 0.0 if abs(avrSWAP_py[23]) < 0.00075: globalDISCON.flagyaw = True globalDISCON.IntYawRef = globalDISCON.IntYaw avrSWAP_py[47] = kiYaw * ( globalDISCON.PosYawRef - avrSWAP_py[36]) + kpYaw * ( globalDISCON.VelYawRef - avrSWAP_py[23]) - YawDamp * avrSWAP_py[23] else: # HERE I CONSIDER PERTURBATIONS ABOUT THE NEW WORKING POSITION globalDISCON.flagyaw = True globalDISCON.IntYawRef = globalDISCON.IntYawRef + globalDISCON.PosYawRef * ElapTime globalDISCON.IntYaw = globalDISCON.IntYaw + avrSWAP_py[ 36] * ElapTime avrSWAP_py[47] = -YawDamp * (avrSWAP_py[23] - 0.0) - YawSpr * ( avrSWAP_py[36] - globalDISCON.PosYawRef) + kpYaw * ( globalDISCON.PosYawRef - avrSWAP_py[36]) + kiYaw * ( globalDISCON.IntYawRef - globalDISCON.IntYaw) #YAW LOGIC BLOCK globalDISCON.LastTime = Time print("globalDISCON.LastTime: ", globalDISCON.LastTime) # INPUTS FOR SUPERCONTROLLER to_SC_py = avrSWAP_py[14] # MEASURED POWER OUTPUT avrSWAP_py = numpy.append(avrSWAP_py, to_SC_py) to_SC_py = avrSWAP_py[36] # ACTUAL YAW ANGLE avrSWAP_py = numpy.append(avrSWAP_py, to_SC_py) # END OF SECTION # WIND SPEED OBSERVER SECTION file = open("Bl1outin.txt", "a+") file.write("%f, %f, %f \n" % (avrSWAP_py[29], avrSWAP_py[68], Time)) file.close() file = open("Bl2outin.txt", "a+") file.write("%f, %f, %f \n" % (avrSWAP_py[30], avrSWAP_py[69], Time)) file.close() file = open("Bl3outin.txt", "a+") file.write("%f, %f, %f \n" % (avrSWAP_py[31], avrSWAP_py[70], Time)) file.close() #file = open("Azimuth.txt","a+") #file.write("%f, %f, %f, %f \n" % (avrSWAP_py[59], avrSWAP_py[20], avrSWAP_py[26], Time)) #file.close() #if from_SC_py == 0: tmp = float(OBSERVER.tmp) #POSG acc = float(OBSERVER.acc) #POSR OBSERVER.y = avrSWAP_py[19] #print("tmp: ", OBSERVER.tmp) #print("acc: ", OBSERVER.acc) #print("y: ", OBSERVER.y) OBSERVER.Qg = avrSWAP_py[22] #print("Qg: ", avrSWAP_py[22]) if numpy.isclose(Time, 0.0): x0 = numpy.array([1.5, 120, 0, 0]) xsol = numpy.array([1.5, 120, 0, 0]) OBSERVER.xsol = xsol xppsolin = numpy.array([0, 0, 1.5, 120]) #print(xsol) Qasol = OBSERVER.Qacalc(xppsolin, xsol, float(OBSERVER.y), float(OBSERVER.tmp)) error = 0.0 errorposg = 0.0 errorposr = 0.0 errorwr = 0.0 errorwg = 0.0 pitch_obs = (avrSWAP_py[3] + avrSWAP_py[32] + avrSWAP_py[33]) * 180 / (3 * numpy.pi) if pitch_obs > 17.9: pitch_obs = 17.9 elif pitch_obs < -10: pitch_obs = -10 num = (2 * Qasol) / (numpy.pi * OBSERVER.rho * (xsol[0]**2) * (OBSERVER.R**5)) tsr_obs = optim.fsolve(OBSERVER.func_impl, 4.5, args=(num, pitch_obs)) vento_obs = xsol[0] * OBSERVER.R / tsr_obs file = open("EXSOL.txt", "a+") file.write("%f, %f, %f, %f, %f \n" % (xsol[0], xsol[1], xsol[2], xsol[3], Time)) file.close() file = open("Azimuth.txt", "a+") file.write("%f, %f, %f, %f \n" % (xsol[2], xsol[0], vento_obs, Time)) file.close() else: x0 = OBSERVER.xsol if numpy.isclose(ElapTime, 0.0): ElapTime = 0.005 #print(OBSERVER.xsolold) #input("ELAP TIME = 0.0 PROBLEM") ts = numpy.linspace(Time - ElapTime, Time, 2) xsol = odeint(OBSERVER.dx_dt, x0, ts, args=(float(OBSERVER.y), float(OBSERVER.tmp))) #print("SOL SHAPE: ", numpy.shape(xsol)) OBSERVER.xsol = xsol[-1, :] OBSERVER.xsolold = numpy.vstack((OBSERVER.xsolold, OBSERVER.xsol)) xppsolin = numpy.gradient(OBSERVER.xsolold, ElapTime, axis=0) #print("SOL: ", xsol) #print("XOLD: ", OBSERVER.xsolold) xppsol = OBSERVER.xpp(xsol[-1, :], float(OBSERVER.y), float(OBSERVER.tmp)) #print("INERTIA: ", xppsol) #print("INERTIA: ", xppsolin[-1,:]) Qasol = OBSERVER.Qacalc(xppsolin[-1, :], xsol[-1, :], float(OBSERVER.y), float(OBSERVER.tmp)) error = (Qasol - (avrSWAP_py[13] / avrSWAP_py[20])) / (avrSWAP_py[13] / avrSWAP_py[20]) errorposg = (OBSERVER.tmp - xsol[-1, 3]) / xsol[-1, 3] errorposr = (OBSERVER.acc - xsol[-1, 2]) / xsol[-1, 2] errorwr = (avrSWAP_py[20] - xsol[-1, 0]) / avrSWAP_py[20] errorwg = (avrSWAP_py[19] - xsol[-1, 1]) / avrSWAP_py[19] pitch_obs = (avrSWAP_py[3] + avrSWAP_py[32] + avrSWAP_py[33]) * 180 / (3 * numpy.pi) if pitch_obs > 17.9: pitch_obs = 17.9 elif pitch_obs < -10: pitch_obs = -10 num = (2 * Qasol) / (numpy.pi * OBSERVER.rho * (xsol[-1, 0]**2) * (OBSERVER.R**5)) tsr_obs = optim.fsolve(OBSERVER.func_impl, 4.5, args=(num, pitch_obs)) vento_obs = xsol[-1, 0] * OBSERVER.R / tsr_obs file = open("EXSOL.txt", "a+") file.write( "%f, %f, %f, %f, %f \n" % (xsol[-1, 0], xsol[-1, 1], xsol[-1, 2], xsol[-1, 3], Time)) file.close() file = open("Azimuth.txt", "a+") file.write("%f, %f, %f, %f \n" % (xsol[-1, 2], xsol[-1, 0], vento_obs, Time)) file.close() if vento_obs > 25: vento_obs = 25 elif vento_obs < 3: vento_obs = 3 file = open("Error.txt", "a+") file.write("%f, %f \n" % (error, Time)) file.close() file = open("ErrorPosg.txt", "a+") file.write("%f, %f \n" % (errorposg, Time)) file.close() file = open("ErrorPosr.txt", "a+") file.write("%f, %f \n" % (errorposr, Time)) file.close() file = open("ErrorWG.txt", "a+") file.write("%f, %f \n" % (errorwg, Time)) file.close() file = open("ErrorWR.txt", "a+") file.write("%f, %f \n" % (errorwr, Time)) file.close() file = open("EWR.txt", "a+") file.write("%f, %f \n" % (avrSWAP_py[20], Time)) file.close() file = open("EWG.txt", "a+") file.write("%f, %f \n" % (avrSWAP_py[19], Time)) file.close() file = open("EPOSG.txt", "a+") file.write("%f, %f \n" % (tmp, Time)) file.close() file = open("EPOSR.txt", "a+") file.write("%f, %f \n" % (acc, Time)) file.close() file = open("EPitch.txt", "a+") file.write("%f, %f, %f \n" % ((avrSWAP_py[3] + avrSWAP_py[32] + avrSWAP_py[33]) * 180 / (3 * numpy.pi), pitch_obs, Time)) file.close() file = open("EWIND.txt", "a+") file.write("%f, %f, %f \n" % (vento_obs, Time, HorWindV)) file.close() file = open("EQasol.txt", "a+") file.write("%f, %f \n" % (Qasol, Time)) file.close() file = open("ENum.txt", "a+") file.write("%f, %f \n" % (num, Time)) file.close() OBSERVER.tmp = float(avrSWAP_py[19] * ElapTime + tmp) OBSERVER.acc = float(avrSWAP_py[20] * ElapTime + acc) #print("ERROR: ", error) #print("Qa: ", Qasol) #print("Qareal: ", avrSWAP_py[13]/avrSWAP_py[20]) #print("POWER: ", avrSWAP_py[13]) #WIND YAW ERROR OBSERVER SECTION blmom1 = numpy.array([avrSWAP_py[29], avrSWAP_py[68]]) blmom2 = numpy.array([avrSWAP_py[30], avrSWAP_py[69]]) blmom3 = numpy.array([avrSWAP_py[31], avrSWAP_py[70]]) N = 1 if numpy.isclose(Time, 0.0): azimuth = numpy.array([ xsol[2], xsol[2] + 2 * numpy.pi / 3, xsol[2] + 4 * numpy.pi / 3 ]) wryaw = xsol[0] globalDISCON.wr_old = wryaw # (1/(2*tauF + Ts)) * ((2*tauF - Ts)*globalDISCON.m_out1f_old + Ts*(m_out1 + globalDISCON.m_out1_old)) globalDISCON.wrf_old = wryaw globalDISCON.azimuth_old = azimuth globalDISCON.azimuthf_old = azimuth m_out1 = 1 m_out2 = 0 m_out3 = 0 m_in1 = 1 m_in2 = 0 m_in3 = 0 yawerrmeas.bl1_old = blmom1 yawerrmeas.bl2_old = blmom2 yawerrmeas.bl3_old = blmom3 yawerrmeas.azimuth_old = azimuth[0] else: #azimuth = (1/(2*tauF + Ts)) * ((2*tauF - Ts)*globalDISCON.azimuthf_old + Ts*(numpy.array([xsol[-1,2], xsol[-1,2] + 2*numpy.pi/3, xsol[-1,2] + 4*numpy.pi/3]) + globalDISCON.azimuth_old)) #wryaw = (1/(2*tauF + Ts)) * ((2*tauF - Ts)*globalDISCON.wrf_old + Ts*(xsol[-1,0] + globalDISCON.wr_old)) azimuth = numpy.array([ xsol[-1, 2], xsol[-1, 2] + 2 * numpy.pi / 3, xsol[-1, 2] + 4 * numpy.pi / 3 ]) wryaw = xsol[-1, 0] globalDISCON.wr_old = xsol[-1, 0] globalDISCON.azimuth_old = numpy.array([ xsol[-1, 2], xsol[-1, 2] + 2 * numpy.pi / 3, xsol[-1, 2] + 4 * numpy.pi / 3 ]) globalDISCON.wrf_old = wryaw globalDISCON.azimuthf_old = azimuth yawerrmeas.bl1_old = numpy.vstack((yawerrmeas.bl1_old, blmom1)) yawerrmeas.bl2_old = numpy.vstack((yawerrmeas.bl2_old, blmom2)) yawerrmeas.bl3_old = numpy.vstack((yawerrmeas.bl3_old, blmom3)) yawerrmeas.azimuth_old = numpy.hstack( (yawerrmeas.azimuth_old, azimuth[0])) #if ((azimuth[0] - 2*N*numpy.pi) > yawerrmeas.azimuth_old[0]) and ((azimuth[0] - 2*N*numpy.pi) > yawerrmeas.azimuth_old[1]): inddel = numpy.where( yawerrmeas.azimuth_old < azimuth[0] - 2 * N * numpy.pi) #print("INDDEL: ", inddel[0]) if inddel[0].size > 1: #print(yawerrmeas.azimuth_old.size) yawerrmeas.bl1_old = numpy.delete(yawerrmeas.bl1_old, [inddel[0][:-2]], 0) yawerrmeas.bl2_old = numpy.delete(yawerrmeas.bl2_old, [inddel[0][:-2]], 0) yawerrmeas.bl3_old = numpy.delete(yawerrmeas.bl3_old, [inddel[0][:-2]], 0) yawerrmeas.azimuth_old = numpy.delete(yawerrmeas.azimuth_old, [inddel[0][:-2]], None) #print(yawerrmeas.azimuth_old.size) #print("DELETED OBJECT") ind = numpy.where( yawerrmeas.azimuth_old > azimuth[0] - 2 * N * numpy.pi) #print("IND: ", ind[0]) a = 0 if ind[0][0] == 0: ind[0][0] = 1 a = 1 blmom1into = numpy.interp(azimuth[0] - 2 * N * numpy.pi, [ yawerrmeas.azimuth_old[ind[0][0] - 1], yawerrmeas.azimuth_old[ind[0][0]] ], [ yawerrmeas.bl1_old[ind[0][0] - 1, 0], yawerrmeas.bl1_old[ind[0][0], 0] ]) blmom1inti = numpy.interp(azimuth[0] - 2 * N * numpy.pi, [ yawerrmeas.azimuth_old[ind[0][0] - 1], yawerrmeas.azimuth_old[ind[0][0]] ], [ yawerrmeas.bl1_old[ind[0][0] - 1, 1], yawerrmeas.bl1_old[ind[0][0], 1] ]) blmom2into = numpy.interp( azimuth[0] - 2 * N * numpy.pi + 2 * numpy.pi / 3, [ yawerrmeas.azimuth_old[ind[0][0] - 1] + 2 * numpy.pi / 3, yawerrmeas.azimuth_old[ind[0][0]] + 2 * numpy.pi / 3 ], [ yawerrmeas.bl2_old[ind[0][0] - 1, 0], yawerrmeas.bl2_old[ind[0][0], 0] ]) blmom2inti = numpy.interp( azimuth[0] - 2 * N * numpy.pi + 2 * numpy.pi / 3, [ yawerrmeas.azimuth_old[ind[0][0] - 1] + 2 * numpy.pi / 3, yawerrmeas.azimuth_old[ind[0][0]] + 2 * numpy.pi / 3 ], [ yawerrmeas.bl2_old[ind[0][0] - 1, 1], yawerrmeas.bl2_old[ind[0][0], 1] ]) blmom3into = numpy.interp( azimuth[0] - 2 * N * numpy.pi + 4 * numpy.pi / 3, [ yawerrmeas.azimuth_old[ind[0][0] - 1] + 4 * numpy.pi / 3, yawerrmeas.azimuth_old[ind[0][0]] + 4 * numpy.pi / 3 ], [ yawerrmeas.bl3_old[ind[0][0] - 1, 0], yawerrmeas.bl3_old[ind[0][0], 0] ]) blmom3inti = numpy.interp( azimuth[0] - 2 * N * numpy.pi + 4 * numpy.pi / 3, [ yawerrmeas.azimuth_old[ind[0][0] - 1] + 4 * numpy.pi / 3, yawerrmeas.azimuth_old[ind[0][0]] + 4 * numpy.pi / 3 ], [ yawerrmeas.bl3_old[ind[0][0] - 1, 1], yawerrmeas.bl3_old[ind[0][0], 1] ]) if a == 1: ind[0][0] = 0 mo10 = numpy.trapz( numpy.hstack((blmom1into, yawerrmeas.bl1_old[ind[0], 0])), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]]))) / (2 * N * numpy.pi) mo1c = numpy.trapz( numpy.multiply( numpy.hstack((blmom1into, yawerrmeas.bl1_old[ind[0], 0])), numpy.cos( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])))), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]]))) / (N * numpy.pi) mo1s = numpy.trapz( numpy.multiply( numpy.hstack((blmom1into, yawerrmeas.bl1_old[ind[0], 0])), numpy.sin( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])))), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]]))) / (N * numpy.pi) mi10 = numpy.trapz( numpy.hstack((blmom1inti, yawerrmeas.bl1_old[ind[0], 1])), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]]))) / (2 * N * numpy.pi) mi1c = numpy.trapz( numpy.multiply( numpy.hstack((blmom1inti, yawerrmeas.bl1_old[ind[0], 1])), numpy.cos( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])))), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]]))) / (N * numpy.pi) mi1s = numpy.trapz( numpy.multiply( numpy.hstack((blmom1inti, yawerrmeas.bl1_old[ind[0], 1])), numpy.sin( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])))), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]]))) / (N * numpy.pi) mo20 = numpy.trapz(numpy.hstack( (blmom2into, yawerrmeas.bl2_old[ind[0], 0])), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3) / (2 * N * numpy.pi) mo2c = numpy.trapz(numpy.multiply( numpy.hstack((blmom2into, yawerrmeas.bl2_old[ind[0], 0])), numpy.cos( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3)), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3) / (N * numpy.pi) mo2s = numpy.trapz(numpy.multiply( numpy.hstack((blmom2into, yawerrmeas.bl2_old[ind[0], 0])), numpy.sin( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3)), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3) / (N * numpy.pi) mi20 = numpy.trapz(numpy.hstack( (blmom2inti, yawerrmeas.bl2_old[ind[0], 1])), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3) / (2 * N * numpy.pi) mi2c = numpy.trapz(numpy.multiply( numpy.hstack((blmom2inti, yawerrmeas.bl2_old[ind[0], 1])), numpy.cos( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3)), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3) / (N * numpy.pi) mi2s = numpy.trapz(numpy.multiply( numpy.hstack((blmom2inti, yawerrmeas.bl2_old[ind[0], 1])), numpy.sin( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3)), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 2 * numpy.pi / 3) / (N * numpy.pi) mo30 = numpy.trapz(numpy.hstack( (blmom3into, yawerrmeas.bl3_old[ind[0], 0])), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3) / (2 * N * numpy.pi) mo3c = numpy.trapz(numpy.multiply( numpy.hstack((blmom3into, yawerrmeas.bl3_old[ind[0], 0])), numpy.cos( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3)), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3) / (N * numpy.pi) mo3s = numpy.trapz(numpy.multiply( numpy.hstack((blmom3into, yawerrmeas.bl3_old[ind[0], 0])), numpy.sin( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3)), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3) / (N * numpy.pi) mi30 = numpy.trapz(numpy.hstack( (blmom3inti, yawerrmeas.bl3_old[ind[0], 1])), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3) / (2 * N * numpy.pi) mi3c = numpy.trapz(numpy.multiply( numpy.hstack((blmom3inti, yawerrmeas.bl3_old[ind[0], 1])), numpy.cos( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3)), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3) / (N * numpy.pi) mi3s = numpy.trapz(numpy.multiply( numpy.hstack((blmom3inti, yawerrmeas.bl3_old[ind[0], 1])), numpy.sin( numpy.hstack((azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3)), x=numpy.hstack( (azimuth[0] - 2 * N * numpy.pi, yawerrmeas.azimuth_old[ind[0]])) + 4 * numpy.pi / 3) / (N * numpy.pi) m_out1 = (mo10 + mo20 + mo30) / 3 m_out2 = (mo1c + mo2c + mo3c) / 3 m_out3 = (mo1s + mo2s + mo3s) / 3 m_in1 = (mi10 + mi20 + mi30) / 3 m_in2 = (mi1c + mi2c + mi3c) / 3 m_in3 = (mi1s + mi2s + mi3s) / 3 m_out1f = (1 / (2 * tauF + Ts)) * ( (2 * tauF - Ts) * globalDISCON.m_out1f_old + Ts * (m_out1 + globalDISCON.m_out1_old)) m_out2f = (1 / (2 * tauF + Ts)) * ( (2 * tauF - Ts) * globalDISCON.m_out2f_old + Ts * (m_out2 + globalDISCON.m_out2_old)) m_out3f = (1 / (2 * tauF + Ts)) * ( (2 * tauF - Ts) * globalDISCON.m_out3f_old + Ts * (m_out3 + globalDISCON.m_out3_old)) m_in1f = (1 / (2 * tauF + Ts)) * ( (2 * tauF - Ts) * globalDISCON.m_in1f_old + Ts * (m_in1 + globalDISCON.m_in1_old)) m_in2f = (1 / (2 * tauF + Ts)) * ( (2 * tauF - Ts) * globalDISCON.m_in2f_old + Ts * (m_in2 + globalDISCON.m_in2_old)) m_in3f = (1 / (2 * tauF + Ts)) * ( (2 * tauF - Ts) * globalDISCON.m_in3f_old + Ts * (m_in3 + globalDISCON.m_in3_old)) globalDISCON.m_out1f_old = m_out1f globalDISCON.m_out1_old = m_out1 globalDISCON.m_out2f_old = m_out2f globalDISCON.m_out2_old = m_out2 globalDISCON.m_out3f_old = m_out3f globalDISCON.m_out3_old = m_out3 globalDISCON.m_in1f_old = m_in1f globalDISCON.m_in1_old = m_in1 globalDISCON.m_in2f_old = m_in2f globalDISCON.m_in2_old = m_in2 globalDISCON.m_in3f_old = m_in3f globalDISCON.m_in3_old = m_in3 #m_yaw_u0 = numpy.array([m_out2/m_out1, m_out3/m_out1, m_in2/m_in1, m_in3/m_in1]) m_yaw_u0 = numpy.array([ m_out2f / m_out1f, m_out3f / m_out1f, m_in2f / m_in1f, m_in3f / m_in1f ]) m_yaw_k1 = numpy.array([1, m_out2, m_out3, m_in2, m_in3]) m_yaw = numpy.hstack((m_yaw_u0, m_yaw_k1)) Tmat = yawerrmeas.Tmat_int(vento_obs) #Tmat = yawerrmeas.Tmat_int(HorWindV) ris_yaw = numpy.dot(Tmat, m_yaw.transpose()) crosswind_NF = wryaw * yawerrmeas.R * ris_yaw[ 0] # VERSION WITHOUT YAW ANGLE #angyaw = numpy.arcsin(crosswind/vento_obs) #crosswind = vento_obs * math.sin(angyaw + avrSWAP_py[36]) vertshear = wryaw * yawerrmeas.R * ris_yaw[1] / vento_obs #vertshear = wryaw*yawerrmeas.R*ris_yaw[1]/HorWindV # FILTERING THE SIGNAL OF CROSS WIND WITH BUTTERWORTH 2nd ORDER FILTER #crosswind = (NNEXY.n1 * (globalDISCON.old_cw + globalDISCON.old2_cw + crosswind_NF) - NNEXY.n2 * globalDISCON.oldf_cw - NNEXY.n3 * globalDISCON.old2f_cw) / NNEXY.d1 crosswind = crosswind_NF if numpy.isclose(Time % 22.5, 0.0): globalDISCON.angyaw_old = globalDISCON.angyaw globalDISCON.angyaw = numpy.arctan(crosswind / vento_obs) if abs(globalDISCON.angyaw - globalDISCON.angyaw_old) < 0.035 and abs( globalDISCON.angyaw) > 0.07: globalDISCON.counterY = globalDISCON.counterY + 1.0 if globalDISCON.counterY >= 2.0: globalDISCON.PosFin = globalDISCON.PosYawRef + globalDISCON.angyaw #globalDISCON.VelYawRef = numpy.sign(globalDISCON.angyaw)*0.0349066/3 globalDISCON.flagyaw = False #globalDISCON.signold = numpy.sign(globalDISCON.PosFin - globalDISCON.PosYawRef) globalDISCON.signold = numpy.sign(globalDISCON.angyaw) else: globalDISCON.counterY = 0.0 file = open("EVALUE.txt", "a+") file.write("%f, %f, %f, %f, %f, %f, %f \n" % (globalDISCON.flagyaw, globalDISCON.PosFin, globalDISCON.VelYawRef, globalDISCON.counterY, globalDISCON.angyaw, globalDISCON.angyaw_old, Time)) file.close() flagind = 0 if logic.counter == 1: toNN_in = numpy.hstack((m_yaw_u0, vento_obs)) toNN = numpy.multiply(toNN_in, 1 / NNEX.normC.T) if toNN.any() > 1: ii = numpy.where(toNN >= 1) toNN[ii] = 1 flagind = 1 crosswindNN = NNEX.NN.forward( toNN) * vento_obs * NNEX.normY # VERSION WITHOUT YAW ANGLE else: crosswindNN = 0.0 file = open("ECROSS.txt", "a+") file.write("%f, %f, %f \n" % (crosswind, crosswindNN, Time)) file.close() file = open("EAzimuth.txt", "a+") file.write("%f, %f, %f, %f \n" % (azimuth[0], azimuth[1], azimuth[2], Time)) file.close() file = open("EMOM.txt", "a+") file.write("%f, %f, %f, %f, %f, %f, %f \n" % (m_out1, m_out2, m_out3, m_in1, m_in2, m_in3, Time)) file.close() file = open("EPOWER.txt", "a+") file.write("%f, %f, %f, %f \n" % (avrSWAP_py[14], avrSWAP_py[13], avrSWAP_py[12], Time)) file.close() to_SC_py = globalDISCON.counterY # ACTUAL YAW ANGLE avrSWAP_py = numpy.append(avrSWAP_py, to_SC_py) to_SC_py = globalDISCON.PosFin # ACTUAL YAW ANGLE avrSWAP_py = numpy.append(avrSWAP_py, to_SC_py) return avrSWAP_py