-
Notifications
You must be signed in to change notification settings - Fork 94
/
controller.py
1064 lines (868 loc) · 45 KB
/
controller.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
# Copyright 2019 NREL
# Licensed under the Apache License, Version 2.0 (the "License"); you may not use
# this file except in compliance with the License. You may obtain a copy of the
# License at http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software distributed
# under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
# CONDITIONS OF ANY KIND, either express or implied. See the License for the
# specific language governing permissions and limitations under the License.
import numpy as np
import os
import datetime
from scipy import interpolate, integrate
from rosco.toolbox.utilities import list_check
from scipy import optimize
# Some useful constants
now = datetime.datetime.now()
pi = np.pi
rad2deg = np.rad2deg(1)
deg2rad = np.deg2rad(1)
rpm2RadSec = 2.0*(np.pi)/60.0
RadSec2rpm = 60/(2.0 * np.pi)
class Controller():
"""
Class Controller used to calculate controller tunings parameters
Methods:
-------
tune_controller
Parameters:
-----------
controller_params: dict
Dictionary containing controller paramaters that need to be defined
"""
def __init__(self, controller_params):
'''
Load controller tuning parameters from input dictionary
'''
print('-----------------------------------------------------------------------------')
print(' Tuning a reference wind turbine controller using NREL\'s ROSCO toolbox ')
print('-----------------------------------------------------------------------------')
# Controller Flags
self.LoggingLevel = controller_params['LoggingLevel']
self.F_LPFType = controller_params['F_LPFType']
self.F_NotchType = controller_params['F_NotchType']
self.IPC_ControlMode = controller_params['IPC_ControlMode']
self.VS_ControlMode = controller_params['VS_ControlMode']
self.VS_ConstPower = controller_params['VS_ConstPower']
self.PC_ControlMode = controller_params['PC_ControlMode']
self.Y_ControlMode = controller_params['Y_ControlMode']
self.SS_Mode = controller_params['SS_Mode']
self.WE_Mode = controller_params['WE_Mode']
self.PS_Mode = controller_params['PS_Mode']
self.SD_Mode = controller_params['SD_Mode']
self.Fl_Mode = controller_params['Fl_Mode']
self.TD_Mode = controller_params['TD_Mode']
self.TRA_Mode = controller_params['TRA_Mode']
self.Flp_Mode = controller_params['Flp_Mode']
self.PA_Mode = controller_params['PA_Mode']
self.PF_Mode = controller_params['PF_Mode']
self.AWC_Mode = controller_params['AWC_Mode']
self.Ext_Mode = controller_params['Ext_Mode']
self.ZMQ_Mode = controller_params['ZMQ_Mode']
self.CC_Mode = controller_params['CC_Mode']
self.StC_Mode = controller_params['StC_Mode']
# Necessary parameters
self.U_pc = list_check(controller_params['U_pc'], return_bool=False)
self.zeta_pc = list_check(controller_params['zeta_pc'], return_bool=False)
self.omega_pc = list_check(controller_params['omega_pc'], return_bool=False)
self.zeta_vs = controller_params['zeta_vs']
self.omega_vs = controller_params['omega_vs']
self.interp_type = controller_params['interp_type']
# Optional parameters with defaults
self.min_pitch = controller_params['min_pitch']
self.max_pitch = controller_params['max_pitch']
self.vs_minspd = controller_params['vs_minspd']
self.ss_vsgain = controller_params['ss_vsgain']
self.ss_pcgain = controller_params['ss_pcgain']
self.ps_percent = controller_params['ps_percent']
self.sd_maxpit = controller_params['sd_maxpit']
self.WS_GS_n = controller_params['WS_GS_n']
self.PC_GS_n = controller_params['PC_GS_n']
self.flp_maxpit = controller_params['flp_maxpit']
self.Kp_ipc1p = controller_params['IPC_Kp1p']
self.Ki_ipc1p = controller_params['IPC_Ki1p']
self.Kp_ipc2p = controller_params['IPC_Kp2p']
self.Ki_ipc2p = controller_params['IPC_Kp2p']
self.IPC_Vramp = controller_params['IPC_Vramp']
self.ZMQ_UpdatePeriod = controller_params['ZMQ_UpdatePeriod']
# Optional parameters without defaults
if self.Flp_Mode > 0:
try:
self.flp_kp_norm = controller_params['flp_kp_norm']
self.flp_tau = controller_params['flp_tau']
except:
raise Exception(
'rosco.toolbox:controller: flp_kp_norm and flp_tau must be set if Flp_Mode > 0')
if self.Fl_Mode > 0:
try:
self.twr_freq = controller_params['twr_freq']
self.ptfm_freq = controller_params['ptfm_freq']
except:
raise Exception('rosco.toolbox:controller: twr_freq and ptfm_freq must be set if Fl_Mode > 0')
# Kp_float direct setting
if 'Kp_float' in controller_params:
self.Kp_float = controller_params['Kp_float']
else:
self.Kp_float = np.array([0])
self.tune_Fl = controller_params['tune_Fl']
else:
self.twr_freq = 0
self.ptfm_freq = 0
# Use critical damping if LPFType = 2
if controller_params['F_LPFType'] == 2:
self.F_LPFDamping = 0.7
else:
self.F_LPFDamping = 0.0
# Filter parameters
self.f_we_cornerfreq = controller_params['filter_params']['f_we_cornerfreq']
self.f_fl_highpassfreq = controller_params['filter_params']['f_fl_highpassfreq']
self.f_ss_cornerfreq = controller_params['filter_params']['f_ss_cornerfreq']
self.f_yawerr = controller_params['filter_params']['f_yawerr']
self.f_sd_cornerfreq = controller_params['filter_params']['f_sd_cornerfreq']
# Open loop parameters: set up and error catching
self.OL_Mode = controller_params['OL_Mode']
self.OL_Filename = controller_params['open_loop']['filename']
self.OL_Ind_Breakpoint = self.OL_Ind_GenTq = self.OL_Ind_YawRate = self.OL_Ind_Azimuth = 0
self.OL_Ind_BldPitch = [0,0,0]
self.OL_Ind_CableControl = [0]
self.OL_Ind_StructControl = [0]
if self.OL_Mode:
ol_params = controller_params['open_loop']
self.OL_Ind_Breakpoint = ol_params['OL_Ind_Breakpoint']
self.OL_Ind_BldPitch = ol_params['OL_Ind_BldPitch']
self.OL_Ind_GenTq = ol_params['OL_Ind_GenTq']
self.OL_Ind_YawRate = ol_params['OL_Ind_YawRate']
self.OL_Ind_Azimuth = ol_params['OL_Ind_Azimuth']
self.OL_Ind_CableControl = ol_params['OL_Ind_CableControl']
self.OL_Ind_StructControl = ol_params['OL_Ind_StructControl']
# Check that file exists because we won't write it
if not os.path.exists(controller_params['open_loop']['filename']):
raise Exception(f'Open-loop control set up, but the open loop file {self.OL_Filename} does not exist')
# Pitch actuator parameters
self.PA_Mode = controller_params['PA_Mode']
self.PA_CornerFreq = controller_params['PA_CornerFreq']
self.PA_Damping = controller_params['PA_Damping']
# Save controller_params for later (direct passthrough)
self.controller_params = controller_params
# Error checking: number of breakpoints
if self.WS_GS_n <= self.PC_GS_n:
raise Exception('Number of WS breakpoints is not greater than pitch control breakpoints')
# Error checking: pitch controller inputs
if list_check(self.U_pc) and \
(list_check(self.omega_pc) or list_check(self.zeta_pc)) and \
not len(self.U_pc) == len(self.omega_pc) == len(self.zeta_pc):
raise Exception(
'U_pc, omega_pc, and zeta_pc are all list-like and are not of equal length')
def tune_controller(self, turbine):
"""
Given a turbine model, tune a controller based on the NREL generic controller tuning process
Parameters:
-----------
turbine : class
Turbine class containing necessary turbine information to accurately tune the controller.
"""
# -------------Load Parameters ------------- #
# Re-define Turbine Parameters for shorthand
J = turbine.J # Total rotor inertial (kg-m^2)
rho = turbine.rho # Air density (kg/m^3)
R = turbine.rotor_radius # Rotor radius (m)
Ar = np.pi*R**2 # Rotor area (m^2)
Ng = turbine.Ng # Gearbox ratio (-)
rated_rotor_speed = turbine.rated_rotor_speed # Rated rotor speed (rad/s)
# ------------- Saturation Limits --------------- #
turbine.max_torque = turbine.rated_torque * self.controller_params['max_torque_factor']
# -------------Define Operation Points ------------- #
TSR_rated = rated_rotor_speed*R/turbine.v_rated # TSR at rated
# separate wind speeds by operation regions
# add one to above rated because we don't use rated in the pitch control gain scheduling
v_below_rated = np.linspace(turbine.v_min,turbine.v_rated, num=self.WS_GS_n-self.PC_GS_n)[:-1] # below rated
v_above_rated = np.linspace(turbine.v_rated,turbine.v_max, num=self.PC_GS_n+1) # above rated
v = np.concatenate((v_below_rated, v_above_rated))
# separate TSRs by operations regions
TSR_below_rated = [min(turbine.TSR_operational, rated_rotor_speed*R/v) for v in v_below_rated] # below rated
TSR_above_rated = rated_rotor_speed*R/v_above_rated # above rated
TSR_op = np.concatenate((TSR_below_rated, TSR_above_rated)) # operational TSRs
# Find expected operational Cp values
Cp_above_rated = turbine.Cp.interp_surface(0,TSR_above_rated[0]) # Cp during rated operation (not optimal). Assumes cut-in bld pitch to be 0
Cp_op_br = np.ones(len(v_below_rated)) * turbine.Cp.max # below rated
Cp_op_ar = Cp_above_rated * (TSR_above_rated/TSR_rated)**3 # above rated
Cp_op = np.concatenate((Cp_op_br, Cp_op_ar)) # operational CPs to linearize around
pitch_initial_rad = turbine.pitch_initial_rad
TSR_initial = turbine.TSR_initial
# initialize variables
pitch_op = np.empty(len(TSR_op))
dCp_beta = np.empty(len(TSR_op))
dCp_TSR = np.empty(len(TSR_op))
dCt_beta = np.empty(len(TSR_op))
dCt_TSR = np.empty(len(TSR_op))
Ct_op = np.empty(len(TSR_op))
# ------------- Find Linearized State "Matrices" ------------- #
# At each operating point
for i in range(len(TSR_op)):
# Find pitch angle as a function of expected operating CP for each TSR operating point
Cp_TSR = np.ndarray.flatten(turbine.Cp.interp_surface(turbine.pitch_initial_rad, TSR_op[i])) # all Cp values for a given tsr
Cp_maxidx = Cp_TSR.argmax()
Cp_op[i] = np.clip(Cp_op[i], np.min(Cp_TSR[Cp_maxidx:]), np.max(Cp_TSR[Cp_maxidx:])) # saturate Cp values to be on Cp surface # Find maximum Cp value for this TSR
f_cp_pitch = interpolate.interp1d(Cp_TSR[Cp_maxidx:],pitch_initial_rad[Cp_maxidx:]) # interpolate function for Cp(tsr) values
# expected operational blade pitch values. Saturates by min_pitch if it exists
if v[i] <= turbine.v_rated and isinstance(self.min_pitch, float): # Below rated & defined min_pitch
pitch_op[i] = min(self.min_pitch, f_cp_pitch(Cp_op[i]))
elif isinstance(self.min_pitch, float): # above rated & defined min_pitch
pitch_op[i] = max(self.min_pitch, f_cp_pitch(Cp_op[i]))
else: # no defined minimum pitch schedule
pitch_op[i] = f_cp_pitch(Cp_op[i])
# Calculate Cp Surface gradients
dCp_beta[i], dCp_TSR[i] = turbine.Cp.interp_gradient(pitch_op[i],TSR_op[i])
dCt_beta[i], dCt_TSR[i] = turbine.Ct.interp_gradient(pitch_op[i],TSR_op[i])
# Thrust
Ct_TSR = np.ndarray.flatten(turbine.Ct.interp_surface(turbine.pitch_initial_rad, TSR_op[i])) # all Cp values for a given tsr
f_ct = interpolate.interp1d(pitch_initial_rad,Ct_TSR)
Ct_op[i] = f_ct(pitch_op[i])
Ct_op[i] = np.clip(Ct_op[i], np.min(Ct_TSR), np.max(Ct_TSR)) # saturate Ct values to be on Ct surface
# Define minimum pitch saturation to be at Cp-maximizing pitch angle if not specifically defined
if not isinstance(self.min_pitch, float):
self.min_pitch = pitch_op[0]
# Full Cx surface gradients
dCp_dbeta = dCp_beta/np.diff(pitch_initial_rad)[0]
dCp_dTSR = dCp_TSR/np.diff(TSR_initial)[0]
dCt_dbeta = dCt_beta/np.diff(pitch_initial_rad)[0]
dCt_dTSR = dCt_TSR/np.diff(TSR_initial)[0]
# Linearized system derivatives, equations from https://wes.copernicus.org/articles/7/53/2022/wes-7-53-2022.pdf
dtau_dbeta = Ng/2*rho*Ar*R*(1/TSR_op)*dCp_dbeta*v**2 # (26)
dtau_dlambda = Ng/2*rho*Ar*R*v**2*(1/(TSR_op**2))*(dCp_dTSR*TSR_op - Cp_op) # (7)
dlambda_domega = R/v/Ng
dtau_domega = dtau_dlambda*dlambda_domega
dlambda_dv = -(TSR_op/v)
Pi_beta = 1/2 * rho * Ar * v**2 * dCt_dbeta
Pi_omega = 1/2 * rho * Ar * R * v * dCt_dTSR
Pi_wind = 1/2 * rho * Ar * v**2 * dCt_dTSR * dlambda_dv + rho * Ar * v * Ct_op
# Second order system coefficients
if not self.VS_ConstPower: # Constant torque above rated
A = dtau_domega/J
else: # Constant power above rated
A = dtau_domega/J
A[-len(v_above_rated)+1:] += Ng**2/J * turbine.rated_power/(Ng**2*rated_rotor_speed**2)
B_tau = -Ng**2/J # Torque input
B_beta = dtau_dbeta/J # Blade pitch input
# Wind Disturbance Input
dtau_dv = (0.5 * rho * Ar * 1/rated_rotor_speed) * (dCp_dTSR*dlambda_dv*v**3 + Cp_op*3*v**2)
B_wind = dtau_dv/J # wind speed input - currently unused
# separate and define below and above rated parameters
A_vs = A[0:len(v_below_rated)] # below rated
A_pc = A[-len(v_above_rated)+1:] # above rated
B_tau = B_tau * np.ones(len(v))
# Resample omega_ and zeta_pc at above rated wind speeds
if not list_check(self.omega_pc) and not list_check(self.zeta_pc):
self.omega_pc_U = self.omega_pc * np.ones(len(v_above_rated[1:]))
self.zeta_pc_U = self.zeta_pc * np.ones(len(v_above_rated[1:]))
else:
if self.interp_type == 'sigma': # sigma interpolation
self.omega_pc_U = multi_sigma(v_above_rated[1:], self.U_pc, self.omega_pc)
self.zeta_pc_U = multi_sigma(v_above_rated[1:], self.U_pc, self.zeta_pc)
else: # standard scipy interpolation types
interp_omega = interpolate.interp1d(self.U_pc, self.omega_pc, kind=self.interp_type, bounds_error=False, fill_value='extrapolate')
interp_zeta = interpolate.interp1d(self.U_pc, self.zeta_pc, kind=self.interp_type, bounds_error=False, fill_value='extrapolate')
self.omega_pc_U = interp_omega(v_above_rated[1:])
self.zeta_pc_U = interp_zeta(v_above_rated[1:])
# -- Find gain schedule --
self.pc_gain_schedule = ControllerTypes()
self.pc_gain_schedule.second_order_PI(self.zeta_pc_U, self.omega_pc_U,A_pc,B_beta[-len(v_above_rated)+1:],linearize=True,v=v_above_rated[1:])
self.vs_gain_schedule = ControllerTypes()
self.vs_gain_schedule.second_order_PI(self.zeta_vs, self.omega_vs,A_vs,B_tau[0:len(v_below_rated)],linearize=False,v=v_below_rated)
# -- Find K for Komega_g^2 --
self.vs_rgn2K = (pi*rho*R**5.0 * turbine.Cp.max * turbine.GBoxEff/100 * turbine.GenEff/100) / \
(2.0 * turbine.Cp.TSR_opt**3 * Ng**3) * self.controller_params['rgn2k_factor']
self.vs_refspd = min(turbine.TSR_operational * turbine.v_rated/R, turbine.rated_rotor_speed) * Ng
# -- Define some setpoints --
# minimum rotor speed saturation limits
if self.vs_minspd:
self.vs_minspd = np.maximum(self.vs_minspd, (turbine.TSR_operational * turbine.v_min / turbine.rotor_radius))
else:
self.vs_minspd = (turbine.TSR_operational * turbine.v_min / turbine.rotor_radius)
self.pc_minspd = self.vs_minspd
# max pitch angle for shutdown
if self.sd_maxpit:
self.sd_maxpit = self.sd_maxpit
else:
self.sd_maxpit = pitch_op[-1]
# Set IPC ramp inputs if not already defined
if max(self.IPC_Vramp) == 0.0:
self.IPC_Vramp = [turbine.v_rated*0.8, turbine.v_rated]
# Store some variables
self.v = v # Wind speed (m/s)
self.v_above_rated = v_above_rated
self.v_below_rated = v_below_rated
# Mod by A. Wright
self.v_for_gs = v[-len(v_above_rated)+1:]
# end
self.pitch_op = pitch_op
self.pitch_op_pc = pitch_op[-len(v_above_rated)+1:]
self.TSR_op = TSR_op
self.A = A
self.B_beta = B_beta
self.B_tau = B_tau
self.B_wind = B_wind
self.omega_op = np.maximum(np.minimum(turbine.rated_rotor_speed, TSR_op*v/R), self.vs_minspd)
self.Pi_omega = Pi_omega
self.Pi_beta = Pi_beta
self.Pi_wind = Pi_wind
# - Might want these to debug -
self.Cp_op = Cp_op
# --- Minimum pitch saturation ---
self.ps_min_bld_pitch = np.ones(len(self.pitch_op)) * self.min_pitch
self.ps = ControllerBlocks()
if self.PS_Mode == 1: # Peak Shaving
self.ps.peak_shaving(self, turbine)
elif self.PS_Mode == 2: # Cp-maximizing minimum pitch saturation
self.ps.min_pitch_saturation(self,turbine)
elif self.PS_Mode == 3: # Peak shaving and Cp-maximizing minimum pitch saturation
self.ps.peak_shaving(self, turbine)
self.ps.min_pitch_saturation(self,turbine)
# --- Floating feedback term ---
if self.Fl_Mode >= 1: # Floating feedback
# Wind speed gain scheduling
self.U_Fl = self.controller_params['U_Fl']
if self.U_Fl: # default is [], only have one Fl_Kp
if type(self.U_Fl) == str:
if self.U_Fl == 'all':
# Mod by A. Wright: get the array of Kp_float values at the values of v-above rated (see self.v_for_gs calculated around line 344).
self.U_Fl = self.v_for_gs
else:
raise Exception("Invalid entry in controller_params for U_Fl, please see schema")
else:
self.U_Fl = np.array([turbine.v_rated * (1.05)])
# If we haven't set Kp_float as a control parameter, we tune it automatically here
if self.tune_Fl:
Kp_float = (dtau_dv/dtau_dbeta) * Ng
if self.Fl_Mode == 2:
Kp_float *= turbine.TowerHt
f_kp = interpolate.interp1d(v,Kp_float)
self.Kp_float = f_kp(self.U_Fl) # get Kp at v_rated + 0.5 m/s
# Make arrays if not
if not np.shape(self.Kp_float):
self.Kp_float = np.array([self.Kp_float])
if not np.shape(self.U_Fl):
self.U_Fl = np.array([self.U_Fl])
# Check size of Kp_float and U_Fl
if len(self.Kp_float) != len(self.U_Fl):
raise Exception('The sizes of Kp_float and U_Fl are not equal, please check your controller_params')
# Turn on the notch filter if floating and not already on
if not self.F_NotchType:
self.F_NotchType = 2
# And check for .yaml input inconsistencies
if self.twr_freq == 0.0 or self.ptfm_freq == 0.0:
print('WARNING: twr_freq and ptfm_freq should be defined for floating turbine control!!')
else:
self.Kp_float = np.array([0.0])
self.U_Fl = np.array([0.0])
# Flap actuation
if self.Flp_Mode >= 1:
self.flp_angle = 0.0
try:
self.tune_flap_controller(turbine)
except AttributeError:
print('ERROR: If Flp_Mode > 0, you need to have blade information loaded in the turbine object.')
raise
except UnboundLocalError:
print('ERROR: You are attempting to tune a flap controller for a blade without flaps!')
raise
else:
self.flp_angle = 0.0
self.Ki_flap = np.array([0.0])
self.Kp_flap = np.array([0.0])
# --- Set up filters ---
self.f_lpf_cornerfreq = turbine.bld_edgewise_freq / 4
# Notch filters
self.f_notch_freqs = []
self.f_notch_beta_nums = []
self.f_notch_beta_dens = []
self.f_notch_gen_inds = []
self.f_notch_twr_inds = []
if self.F_NotchType:
if self.Flp_Mode:
self.f_notch_freqs.append(turbine.bld_flapwise_freq)
self.f_notch_beta_nums.append(0.0)
self.f_notch_beta_dens.append(0.50)
else:
self.f_notch_freqs.append(self.twr_freq)
self.f_notch_beta_nums.append(0.0)
self.f_notch_beta_dens.append(0.25)
if self.F_NotchType == 1 or self.F_NotchType == 3:
self.f_notch_gen_inds.append(1)
elif self.F_NotchType == 2:
self.f_notch_twr_inds.append(1)
# --- Direct input passthrough ---
if 'f_lpf_cornerfreq' in self.controller_params['filter_params']:
self.f_lpf_cornerfreq = self.controller_params['filter_params']['f_lpf_cornerfreq']
def tune_flap_controller(self,turbine):
'''
Tune controller for distributed aerodynamic control
Parameters:
-----------
turbine : class
Turbine class containing necessary turbine information to accurately tune the controller.
'''
# Find blade aerodynamic coefficients
v_rel = []
phi_vec = []
for i, _ in enumerate(self.v):
turbine.cc_rotor.induction_inflow=True
# Axial and tangential inductions
try:
a, ap, _, _, _ = turbine.cc_rotor.distributedAeroLoads(
self.v[i], self.omega_op[i], self.pitch_op[i], 0.0)
except ValueError:
loads, _ = turbine.cc_rotor.distributedAeroLoads(
self.v[i], self.omega_op[i], self.pitch_op[i], 0.0)
a = loads['a'] # Axial induction factor
ap = loads['ap'] # Tangential induction factor
# Relative windspeed along blade span
v_rel.append([np.sqrt(self.v[i]**2*(1-a)**2 + self.omega_op[i]**2*turbine.span**2*(1-ap)**2)])
# Inflow wind direction
phi_vec.append(self.pitch_op[i] + turbine.twist*deg2rad)
# Lift and drag coefficients
num_af = len(turbine.af_data) # number of airfoils
Cl0 = np.zeros(num_af)
Cd0 = np.zeros(num_af)
Clp = np.zeros(num_af)
Cdp = np.zeros(num_af)
Clm = np.zeros(num_af)
Cdm = np.zeros(num_af)
for i,section in enumerate(turbine.af_data):
# assume airfoil section as AOA of zero for slope calculations
a0_ind = section[0]['Alpha'].index(np.min(np.abs(section[0]['Alpha'])))
# Coefficients
# - If the flap exists in this blade section, define Cx-plus,-minus,-neutral(0)
# - IF teh flap does not exist in this blade section, Cx matrix is all the same value
if section[0]['NumTabs'] == 3: # sections with 3 flaps
Clm[i,] = section[0]['Cl'][a0_ind]
Cdm[i,] = section[0]['Cd'][a0_ind]
Cl0[i,] = section[1]['Cl'][a0_ind]
Cd0[i,] = section[1]['Cd'][a0_ind]
Clp[i,] = section[2]['Cl'][a0_ind]
Cdp[i,] = section[2]['Cd'][a0_ind]
Ctrl_flp = float(section[2]['Ctrl'])
else: # sections without 3 flaps
Cl0[i,] = Clp[i,] = Clm[i,] = section[0]['Cl'][a0_ind]
Cd0[i,] = Cdp[i,] = Cdm[i,] = section[0]['Cd'][a0_ind]
Ctrl = float(section[0]['Ctrl'])
# Find lift and drag coefficient slopes w.r.t. flap angle
Kcl = (Clp - Cl0)/( (Ctrl_flp-Ctrl)*deg2rad )
Kcd = (Cdp - Cd0)/( (Ctrl_flp-Ctrl)*deg2rad )
# Find integrated constants
self.kappa = np.zeros(len(v_rel)) # "flap efficacy term"
C1 = np.zeros(len(v_rel))
C2 = np.zeros(len(v_rel))
for i, (v_sec,phi) in enumerate(zip(v_rel, phi_vec)):
C1[i] = integrate.trapezoid(0.5 * turbine.rho * turbine.chord * v_sec[0]**2 * turbine.span * Kcl * np.cos(phi))
C2[i] = integrate.trapezoid(0.5 * turbine.rho * turbine.chord * v_sec[0]**2 * turbine.span * Kcd * np.sin(phi))
self.kappa[i]=C1[i]+C2[i]
# PI Gains
if (self.flp_kp_norm == 0 or self.flp_tau == 0) or (not self.flp_kp_norm or not self.flp_tau):
raise ValueError('flp_kp_norm and flp_tau must be nonzero for Flp_Mode >= 1')
self.Kp_flap = self.flp_kp_norm / self.kappa
self.Ki_flap = self.flp_kp_norm / self.kappa / self.flp_tau
class ControllerBlocks():
'''
Class ControllerBlocks defines tuning parameters for additional controller features or "blocks"
Methods:
--------
peak_shaving
'''
def __init__(self):
pass
def peak_shaving(self,controller, turbine):
'''
Define minimum blade pitch angle for peak shaving routine based on a maximum allowable thrust
Parameters:
-----------
controller: class
Controller class containing controller operational information
turbine: class
Turbine class containing necessary wind turbine information for controller tuning
'''
# Re-define Turbine Parameters for shorthand
rho = turbine.rho # Air density (kg/m^3)
R = turbine.rotor_radius # Rotor radius (m)
A = np.pi*R**2 # Rotor area (m^2)
# Initialize some arrays
Ct_op = np.empty(len(controller.TSR_op),dtype='float64')
Ct_max = np.empty(len(controller.TSR_op),dtype='float64')
# Find unshaved rotor thrust coefficients at each TSR
for i in range(len(controller.TSR_op)):
Ct_op[i] = turbine.Ct.interp_surface(controller.pitch_op[i],controller.TSR_op[i])
# Thrust vs. wind speed
T = 0.5 * rho * A * controller.v**2 * Ct_op
# Define minimum max thrust and initialize pitch_min
Tmax = controller.ps_percent * np.max(T)
pitch_min = np.ones(len(controller.pitch_op)) * controller.min_pitch
# Modify pitch_min if max thrust exceeds limits
for i in range(len(controller.TSR_op)):
# Find Ct values for operational TSR
Ct_tsr = turbine.Ct.interp_surface(turbine.pitch_initial_rad,controller.TSR_op[i])
# Define max Ct values
Ct_max[i] = Tmax/(0.5 * rho * A * controller.v[i]**2)
if T[i] > Tmax:
Ct_op[i] = Ct_max[i]
else:
# TSR_below_rated = np.minimum(np.max(TSR_above_rated), TSR_below_rated)
Ct_max[i] = np.minimum( np.max(Ct_tsr), Ct_max[i])
# Define minimum pitch angle
# - find min(\beta) so that Ct <= Ct_max and \beta > \beta_fine at each operational TSR
f_pitch_min = interpolate.interp1d(Ct_tsr, turbine.pitch_initial_rad, kind='linear', bounds_error=False, fill_value=(turbine.pitch_initial_rad[0],turbine.pitch_initial_rad[-1]))
pitch_min[i] = max(controller.min_pitch, f_pitch_min(Ct_max[i]))
# Save to controller object
controller.ps_min_bld_pitch = pitch_min
# save some outputs for analysis or future work
self.Tshaved = 0.5 * rho * A * controller.v**2 * Ct_op
self.pitch_min = pitch_min
self.v = controller.v
self.Ct_max = Ct_max
self.Ct_op = Ct_op
self.T = T
def min_pitch_saturation(self, controller, turbine):
'''
Minimum pitch saturation in low wind speeds to maximize power capture
Parameters:
-----------
controller: class
Controller class containing controller operational information
turbine: class
Turbine class containing necessary wind turbine information for controller tuning
'''
# Find TSR associated with minimum rotor speed
TSR_at_minspeed = (controller.pc_minspd) * turbine.rotor_radius / controller.v_below_rated
# For each below rated wind speed operating point
for i in range(len(TSR_at_minspeed)):
if TSR_at_minspeed[i] > controller.TSR_op[i]:
controller.TSR_op[i] = TSR_at_minspeed[i]
# Initialize some arrays
Cp_op = np.empty(len(turbine.pitch_initial_rad),dtype='float64')
min_pitch = np.empty(len(TSR_at_minspeed),dtype='float64')
# ------- Find Cp-maximizing minimum pitch schedule ---------
# Cp coefficients at below-rated tip speed ratios
Cp_op = turbine.Cp.interp_surface(turbine.pitch_initial_rad,TSR_at_minspeed[i])
# Setup and run small optimization problem to find blade pitch angle that maximizes Cp at a given TSR
# - Finds \beta to satisfy max( Cp(\beta,TSR_op) )
f_pitch_min = interpolate.interp1d(turbine.pitch_initial_rad, -Cp_op, kind='quadratic', bounds_error=False, fill_value=(turbine.pitch_initial_rad[0],turbine.pitch_initial_rad[-1]))
res = optimize.minimize(f_pitch_min, 0.0)
min_pitch[i] = res.x[0]
# modify existing minimum pitch schedule
controller.ps_min_bld_pitch[i] = np.maximum(controller.ps_min_bld_pitch[i], min_pitch[i])
# Save Cp_op
controller.Cp_op[i] = -res.fun
else:
return
class ControllerTypes():
'''
Class ControllerTypes used to define any types of controllers that can be tuned.
Generally, calculates gains based on some pre-defined tuning parameters.
Methods:
--------
second_order_PI
'''
def __init__(self):
pass
def second_order_PI(self,zeta,om_n,A,B,linearize=False,v=None):
'''
Define proportional integral gain schedule for a closed
loop system with a standard second-order form.
Parameters:
-----------
zeta : list of floats (-)
Desired damping ratio with breakpoints at v
om_n : list of floats (rad/s)
Desired natural frequency with breakpoints at v
A : array_like (1/s)
Plant poles (state transition matrix)
B : array_like (varies)
Plant numerators (input matrix)
linearize : bool, optional
If 'True', find a gain scheduled based on a linearized plant.
v : array_like (m/s)
Wind speeds for linearized plant model, if desired.
'''
# Linearize system coefficients w.r.t. wind speed if desired
if linearize:
pA = np.polyfit(v,A,1)
pB = np.polyfit(v,B,1)
A = pA[0]*v + pA[1]
B = pB[0]*v + pB[1]
# Calculate gain schedule
self.Kp = 1/B * (2*zeta*om_n + A)
self.Ki = om_n**2/B
class OpenLoopControl(object):
'''
Open loop controls for
- blade_pitch
- generator_torque
- nacelle_yaw and nacelle_yaw_rate
Please see Examples/example_14.py for an example on how to use this class.
'''
def __init__(self, **kwargs):
self.dt = 0.05
self.t_max = 200
# Optional population class attributes from key word arguments
for (k, w) in kwargs.items():
try:
setattr(self, k, w)
except:
pass
self.ol_timeseries = {}
self.ol_timeseries['time'] = np.arange(0,self.t_max,self.dt)
self.allowed_controls = ['blade_pitch','generator_torque','nacelle_yaw','nacelle_yaw_rate','cable_control','struct_control']
def const_timeseries(self,control,value):
self.ol_timeseries[control] = value * np.ones(len(self.ol_timeseries['time']))
def interp_timeseries(self,control,breakpoints,values,method='sigma'):
# Error checking
if not list_check(breakpoints) or len(breakpoints) == 1:
raise Exception('Open loop breakpoints are not a list with length greater than 1')
if not list_check(values) or len(values) == 1:
raise Exception('Open loop values are not a list with length greater than 1')
if len(breakpoints) != len(values):
raise Exception('Open loop breakpoints and values do not have the same length')
# Check if control in allowed controls, cable_control_* is in cable_control
if not any([ac in control for ac in self.allowed_controls]):
raise Exception(f'Open loop control of {control} is not allowed')
else:
# Finally interpolate
if method == 'sigma':
self.ol_timeseries[control] = multi_sigma(self.ol_timeseries['time'],breakpoints,values)
elif method == 'linear':
interp_fcn = interpolate.interp1d(breakpoints,values,fill_value=values[-1],bounds_error=False)
self.ol_timeseries[control] = interp_fcn(self.ol_timeseries['time'])
elif method == 'cubic':
interp_fcn = interpolate.interp1d(breakpoints,values,kind='cubic',fill_value=values[-1],bounds_error=False)
self.ol_timeseries[control] = interp_fcn(self.ol_timeseries['time'])
else:
raise Exception(f'Open loop interpolation method {method} not supported')
if control == 'nacelle_yaw':
self.compute_yaw_rate()
def sine_timeseries(self,control,amplitude,period):
if period <= 0:
raise Exception('Open loop sine input period is <= 0')
if control not in self.allowed_controls:
raise Exception(f'Open loop control of {control} is not allowed')
else:
self.ol_timeseries[control] = amplitude * np.sin(2 * np.pi * self.ol_timeseries['time'] / period)
if control == 'nacelle_yaw':
self.compute_yaw_rate()
def compute_yaw_rate(self):
self.ol_timeseries['nacelle_yaw_rate'] = np.concatenate(([0],np.diff(self.ol_timeseries['nacelle_yaw'])))/self.dt
def plot_timeseries(self):
'''
Debugging script for showing open loop timeseries
'''
import matplotlib.pyplot as plt
fig, ax = plt.subplots(len(self.ol_timeseries)-1,1)
i_ax = -1
for ol_input in self.ol_timeseries:
if ol_input != 'time':
i_ax += 1
if len(self.ol_timeseries)-1 == 1:
ax.plot(self.ol_timeseries['time'],self.ol_timeseries[ol_input])
ax.set_ylabel(ol_input)
else:
ax[i_ax].plot(self.ol_timeseries['time'],self.ol_timeseries[ol_input])
ax[i_ax].set_ylabel(ol_input)
return fig, ax
def write_input(self,ol_filename):
'''
Write open loop control input
Return open_loop dict for control params
'''
ol_timeseries = self.ol_timeseries
# Init indices
OL_Ind_Breakpoint = 1
OL_Ind_Azimuth = OL_Ind_GenTq = OL_Ind_YawRate = 0
OL_Ind_BldPitch = 3*[0]
OL_Ind_CableControl = []
OL_Ind_StructControl = []
ol_index_counter = 0 # start input index at 2
# Write time first, initialize OL matrix
if 'time' in ol_timeseries:
ol_control_array = ol_timeseries['time']
Ind_Breakpoint = 1
else:
raise Exception('WARNING: no time index for open loop control. This is only index currently supported')
for channel in ol_timeseries:
# increment index counter first for 1-indexing in input file
ol_index_counter += 1
# skip writing for certain channels
skip_write = False
# Set open loop index based on name
if channel == 'time':
OL_Ind_Breakpoint = ol_index_counter
skip_write = True
elif channel == 'blade_pitch': # collective blade pitch
OL_Ind_BldPitch = 3 * [ol_index_counter]
elif channel == 'generator_torque':
OL_Ind_GenTq = ol_index_counter
elif channel == 'nacelle_yaw_rate':
OL_Ind_YawRate = ol_index_counter
elif channel == 'nacelle_yaw':
ol_index_counter -= 1 # don't increment counter
skip_write = True
elif channel == 'blade_pitch1':
OL_Ind_BldPitch[0] = ol_index_counter
elif channel == 'blade_pitch2':
OL_Ind_BldPitch[1] = ol_index_counter
elif channel == 'blade_pitch3':
OL_Ind_BldPitch[2] = ol_index_counter
elif channel == 'azimuth':
OL_Ind_Azimuth = ol_index_counter
elif 'cable_control' in channel or 'struct_control' in channel:
skip_write = True
ol_index_counter -= 1 # don't increment counter
# append open loop input array for non-ptfm channels
if not skip_write:
ol_control_array = np.c_[ol_control_array,ol_timeseries[channel]]
ol_index_counter += 1 # Increment counter so it's 1 more than time, just like above in each iteration
# Cable control
is_cable_chan = np.array(['cable_control' in ol_chan for ol_chan in ol_timeseries.keys()])
if any(is_cable_chan):
# if any channels are cable_control_*
n_cable_chan = np.sum(is_cable_chan)
cable_chan_names = np.array(list(ol_timeseries.keys()))[is_cable_chan]
# Let's assume they are 1-indexed and all there, otherwise a key error will be thrown
for cable_chan in cable_chan_names:
ol_control_array = np.c_[ol_control_array,ol_timeseries[cable_chan]]
OL_Ind_CableControl.append(ol_index_counter)
ol_index_counter += 1
# Struct control
is_struct_chan = ['struct_control' in ol_chan for ol_chan in ol_timeseries.keys()]
if any(is_struct_chan):
# if any channels are struct_control_*
n_struct_chan = np.sum(np.array(is_struct_chan))
# Let's assume they are 1-indexed and all there, otherwise a key error will be thrown
for i_chan in range(1,n_struct_chan+1):
ol_control_array = np.c_[ol_control_array,ol_timeseries[f'struct_control_{i_chan}']]
OL_Ind_StructControl.append(ol_index_counter)
ol_index_counter += 1
# Open file
if not os.path.exists(os.path.dirname(os.path.abspath(ol_filename))):
os.makedirs(os.path.dirname(os.path.abspath(ol_filename)))
with open(ol_filename,'w') as f:
# Write header
headers = [''] * ol_index_counter
units = [''] * ol_index_counter
header_line = '!\tTime'
unit_line = '!\t(sec.)'
headers[0] = 'Time'
units[0] = 'sec.'
if OL_Ind_GenTq:
headers[OL_Ind_GenTq-1] = 'GenTq'
units[OL_Ind_GenTq-1] = '(Nm)'
if OL_Ind_YawRate:
headers[OL_Ind_YawRate-1] = 'YawRate'
units[OL_Ind_YawRate-1] = '(rad/s)'
if OL_Ind_Azimuth:
headers[OL_Ind_Azimuth-1] = 'Azimuth'
units[OL_Ind_Azimuth-1] = '(rad)'
if any(OL_Ind_BldPitch):
if all_same(OL_Ind_BldPitch):
headers[OL_Ind_BldPitch[0]-1] = 'BldPitch123'
units[OL_Ind_BldPitch[0]-1] = '(rad)'
else:
headers[OL_Ind_BldPitch[0]-1] = 'BldPitch1'
units[OL_Ind_BldPitch[0]-1] = '(rad)'
headers[OL_Ind_BldPitch[1]-1] = 'BldPitch2'
units[OL_Ind_BldPitch[1]-1] = '(rad)'
headers[OL_Ind_BldPitch[2]-1] = 'BldPitch3'
units[OL_Ind_BldPitch[2]-1] = '(rad)'
if OL_Ind_CableControl:
for i_chan in range(1,n_cable_chan+1):
header_line += f'\t\tCable{i_chan}'
unit_line += '\t\t(m)'
else:
OL_Ind_CableControl = [0]
if OL_Ind_StructControl:
for i_chan in range(1,n_struct_chan+1):
header_line += f'\t\tStruct{i_chan}'
unit_line += '\t\t(m)'
else:
OL_Ind_StructControl = [0]
# Join headers and units
header_line = '!' + '\t\t'.join(headers) + '\n'
unit_line = '!' + '\t\t'.join(units) + '\n'
f.write(header_line)
f.write(unit_line)
# Write lines
for ol_line in ol_control_array:
line = ''.join(['{:<10.8f}\t'.format(val) for val in ol_line]) + '\n'
f.write(line)
# Output open_loop dict for control params
open_loop = {}
open_loop['filename'] = ol_filename
open_loop['OL_Ind_Breakpoint'] = OL_Ind_Breakpoint
open_loop['OL_Ind_BldPitch'] = OL_Ind_BldPitch
open_loop['OL_Ind_GenTq'] = OL_Ind_GenTq
open_loop['OL_Ind_YawRate'] = OL_Ind_YawRate
open_loop['OL_Ind_Azimuth'] = OL_Ind_Azimuth
open_loop['OL_Ind_CableControl'] = OL_Ind_CableControl
open_loop['OL_Ind_StructControl'] = OL_Ind_StructControl
return open_loop
# ----------- Helper Functions -----------
def sigma(tt,t0,t1,y0=0,y1=1):
'''
generates timeseries for a smooth transition from y0 to y1 from x0 to x1
Parameters:
-----------
tt: List-like
time indices
t0: float
start time
t1: float
end time
y0: float
start output
y1:
end output