-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathmain.m
More file actions
92 lines (83 loc) · 10.9 KB
/
main.m
File metadata and controls
92 lines (83 loc) · 10.9 KB
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
%% nachaklni nastroiki
clc,clear,close all;
%% zadavane na parametri za pri modelirane
control_a=[-1,-0.975000000000000,-0.950000000000000,-0.925000000000000,-0.900000000000000,-0.875000000000000,-0.850000000000000,-0.825000000000000,-0.800000000000000,-0.775000000000000,-0.750000000000000,-0.725000000000000,-0.700000000000000,-0.675000000000000,-0.650000000000000,-0.625000000000000,-0.600000000000000,-0.575000000000000,-0.550000000000000,-0.525000000000000,-0.500000000000000,-0.475000000000000,-0.450000000000000,-0.425000000000000,-0.400000000000000,-0.375000000000000,-0.350000000000000,-0.325000000000000,-0.300000000000000,-0.275000000000000,-0.250000000000000,-0.225000000000000,-0.200000000000000,-0.175000000000000,-0.150000000000000,-0.125000000000000,-0.100000000000000,-0.0750000000000000,-0.0500000000000000,-0.0250000000000000,0,0.0250000000000000,0.0500000000000000,0.0750000000000000,0.100000000000000,0.125000000000000,0.150000000000000,0.175000000000000,0.200000000000000,0.225000000000000,0.250000000000000,0.275000000000000,0.300000000000000,0.325000000000000,0.350000000000000,0.375000000000000,0.400000000000000,0.425000000000000,0.450000000000000,0.475000000000000,0.500000000000000,0.525000000000000,0.550000000000000,0.575000000000000,0.600000000000000,0.625000000000000,0.650000000000000,0.675000000000000,0.700000000000000,0.725000000000000,0.750000000000000,0.775000000000000,0.800000000000000,0.825000000000000,0.850000000000000,0.875000000000000,0.900000000000000,0.925000000000000,0.950000000000000,0.975000000000000,1;];
control_p=[-1,-0.975000000000000,-0.950000000000000,-0.925000000000000,-0.900000000000000,-0.875000000000000,-0.850000000000000,-0.825000000000000,-0.800000000000000,-0.775000000000000,-0.750000000000000,-0.725000000000000,-0.700000000000000,-0.675000000000000,-0.650000000000000,-0.625000000000000,-0.600000000000000,-0.575000000000000,-0.550000000000000,-0.525000000000000,-0.500000000000000,-0.475000000000000,-0.450000000000000,-0.425000000000000,-0.400000000000000,-0.375000000000000,-0.350000000000000,-0.325000000000000,-0.300000000000000,-0.275000000000000,-0.250000000000000,-0.225000000000000,-0.200000000000000,-0.175000000000000,-0.150000000000000,-0.125000000000000,-0.100000000000000,-0.0750000000000000,-0.0500000000000000,-0.0250000000000000,0,0.0250000000000000,0.0500000000000000,0.0750000000000000,0.100000000000000,0.125000000000000,0.150000000000000,0.175000000000000,0.200000000000000,0.225000000000000,0.250000000000000,0.275000000000000,0.300000000000000,0.325000000000000,0.350000000000000,0.375000000000000,0.400000000000000,0.425000000000000,0.450000000000000,0.475000000000000,0.500000000000000,0.525000000000000,0.550000000000000,0.575000000000000,0.600000000000000,0.625000000000000,0.650000000000000,0.675000000000000,0.700000000000000,0.725000000000000,0.750000000000000,0.775000000000000,0.800000000000000,0.825000000000000,0.850000000000000,0.875000000000000,0.900000000000000,0.925000000000000,0.950000000000000,0.975000000000000,1;];
force_a=[-1.11662323915155,-1.09378407342940,-1.07265330175950,-1.04325193963191,-1.01132720072323,-0.977223749190414,-0.938313974120250,-0.915271484375000,-0.878089162348878,-0.843089432210708,-0.808883264518567,-0.764209831066494,-0.722158864286485,-0.679419665506261,-0.643924317249568,-0.610622608349525,-0.566994046173359,-0.526776706201425,-0.495851299384715,-0.457681114394430,-0.413805712974957,-0.389259586841537,-0.353816524314551,-0.318696958657168,-0.287696580850605,-0.255048612235535,-0.227182257933938,-0.197140901608377,-0.168667442519430,-0.144984958576209,-0.124217404064119,-0.101658725982297,-0.0816850712435233,-0.0636177373434801,-0.0471739596826425,-0.0331590228303109,-0.0244596893890328,-0.0162074832685665,-0.00541132340241796,-0.00395584048467185,-1.55339216321175e-05,0.000125890544041459,0.00293734482944733,0.00703098013816926,0.0122942978195164,0.0176934740662781,0.0254305814173143,0.0346500735373489,0.0441126943005182,0.0567768040263385,0.0702119596016839,0.0760190488719776,0.0911021325831174,0.104758321864206,0.124595527714810,0.137806950966105,0.148332321351468,0.164687887926382,0.183697967265760,0.202922924087867,0.208661451586788,0.235725388601036,0.252848206093480,0.263888337920984,0.282229412915587,0.314479672657599,0.331760106325561,0.342049431940846,0.376631382232297,0.399946095099309,0.437765206714162,0.442304535702720,0.462774567546416,0.494541268620466,0.511848182480570,0.523759715025907,0.551435310745898,0.571018846475605,0.596035206849093,0.618583933775907,0.649258706417314;];
force_p=[-0.714170782059585,-0.698541481136658,-0.676359428972366,-0.657306593399180,-0.636149510200777,-0.615949677515112,-0.602138570676813,-0.585879142379102,-0.565408182885363,-0.543655801354706,-0.527755120628238,-0.502405059234672,-0.485276085519214,-0.468134124703152,-0.445933772533463,-0.427151136118307,-0.408462867012090,-0.386931266191710,-0.367853637062824,-0.349068470693005,-0.324248856460492,-0.305863338595639,-0.289661424600605,-0.264967802110320,-0.251039558371114,-0.233093409299439,-0.211345413023532,-0.189864411296416,-0.167559403335492,-0.150489967886442,-0.132682966321244,-0.112475290776123,-0.0958084554458117,-0.0785459844559585,-0.0611414143458549,-0.0434235515166235,-0.0307778261280224,-0.0175960033462867,-0.00664694988665803,-8.85484132124506e-05,-1.31726306131176e-05,0.00108963460708116,0.00830363166558722,0.0205083016245682,0.0372826600280656,0.0556504682102763,0.0777436346340674,0.102116964863990,0.127878228222150,0.155139080041019,0.182927427407168,0.214085927380181,0.243558802892919,0.276369784920121,0.308195518269646,0.339835131287781,0.374653969667530,0.409305578044042,0.443215825372409,0.474525785297928,0.511802896292099,0.548199583063472,0.583849511550087,0.618635544851036,0.653574809072755,0.683814092184802,0.719783891272669,0.743332709277850,0.784228330094991,0.803452359267055,0.826563579447323,0.856263391893351,0.880490322080095,0.905606278335492,0.936119555537565,0.962942893863342,0.978395603950777,0.989660058425086,0.999002928000864,1.00736645212651,1.02442492443869;];
rpm_a=[-7339.45782001209,-7212.46690016528,-7085.45931302586,-6928.76328688402,-6788.57281024633,-6657.59465144230,-6511.41380896936,-6365.92031625604,-6207.93597881609,-6041.64663461541,-5911.49080716649,-5761.94058931792,-5622.99053485574,-5477.24984975964,-5308.98813100961,-5148.14194899341,-4993.59811636119,-4838.25096717246,-4657.71766075724,-4490.92806302583,-4332.56953312800,-4153.59004094051,-3981.78757887621,-3798.42599722058,-3616.65719839245,-3425.04671536962,-3236.48705115689,-3046.65550818810,-2842.09148700420,-2651.03642390325,-2432.69606370190,-2221.66560246394,-1994.31175818811,-1773.93564077524,-1537.57112943203,-1286.92626953123,-1034.29612379809,-766.855797400854,-497.395442082297,-201.089712289693,-9.85224797175578,176.811570387611,476.338547926701,748.300170898416,1017.51380333537,1274.81736403251,1528.65929236775,1769.17771559497,2003.15199631911,2232.54371056188,2442.50840407150,2682.82282902644,2882.65146108778,3088.79840557393,3298.89221191406,3486.94152832027,3703.32852877101,3882.69348144536,4068.55539175192,4264.87919734071,4452.71559495185,4621.17826021632,4782.78667743374,4978.75671386709,5156.26361553488,5295.29630220852,5475.22559532753,5656.39061561005,5822.12430513828,5951.17187500000,6105.73636568491,6283.36299015915,6449.69764122601,6590.63485952504,6746.62475585916,6915.86679311890,7028.57994666469,7157.30121319133,7332.44675856368,7488.25002817038,7563.48595252389;];
rpm_p=[-3941.33990805745,-3812.34781047405,-3751.11151919774,-3716.53325007516,-3652.15015900441,-3605.21060614596,-3541.61821912219,-3501.87845652477,-3434.16395565290,-3361.65746230900,-3323.64747018525,-3262.37352633531,-3200.40694372361,-3113.97714681560,-3066.79116629220,-2985.72573717380,-2938.26747796475,-2854.00073711688,-2797.89727840112,-2739.44446768238,-2641.03778047718,-2564.22034843818,-2474.92614603819,-2398.72604761367,-2312.76690787646,-2203.69161681418,-2111.88368363814,-2025.87649472111,-1916.57079238712,-1808.31851914770,-1700.81929738149,-1583.07014589823,-1436.64012988966,-1312.93597099109,-1159.60626135342,-1003.00583561542,-827.249522531483,-632.475849854098,-417.825271144055,-131.599678804250,-12.6526696976537,111.061323828355,396.521220173868,611.307797732044,818.484059580521,988.289299100153,1155.28492605217,1301.68712411454,1436.53110601965,1562.36151137272,1681.59372467539,1794.19768006651,1907.80954427652,2017.66991959864,2113.14813573876,2213.00248010454,2301.04571789298,2377.92833946245,2463.59047345069,2543.08432910270,2626.60813409407,2701.33444334797,2778.43230276394,2843.35629834439,2916.67737983047,2995.35608536158,3058.64528487056,3118.35187009445,3179.05933940331,3243.22129185106,3317.14172363282,3374.58353464978,3419.17148759037,3480.56513290589,3539.80134194823,3604.31509306933,3632.26538526984,3707.08526397923,3751.46921864755,3806.30176250743,3857.44884997813;];
%% pid control
Ts = 0.1;
% zigler-nickolos
% Ku - kritichen koeficient na obekta
% Tu - period na kolebaniqta v obekta
% Ap - amplituda na kolebaniqta v obekta
% h - amplituda na kolebaniqta v releto
% azimut
ha=0.3;
Apa=(1.65-.5)/2;
Tua=29.5-22;
Kua=4*ha/pi/Apa;
% pitch
hp=0.2;
App=(0.615--0.6995)/2;
Tup=56-53.5;
Kup=4*hp/pi/App;
% http://www.mstarlabs.com/control/znrule.html Some Overshoot
% Kp = 0.33*Ku;
% Ti = 0.5*Tu;
% Td = 0.33*Tu;
% Kpa = 0.33*Kua;
% Tia = 0.5*Tua;
% Tda = 0.33*Tua;
%
% Kpa = 0.33*Kua;
% Tia = 0.5*Tua;
% Tda = 0.33*Tua;
% azimut
% http://www.mstarlabs.com/control/znrule.html Pessen Integral Rule
Kpa = 0.7*Kua;
Tia = 0.4*Tua;% napaleno e za da moje da se poluchi "dobro" upravlenie
Tda = 0.15*Tua;
% http://www.mstarlabs.com/control/znrule.html Some Overshoot
% Kp = 0.33*Ku;
% Ti = 0.5*Tu;
% Td = 0.33*Tu;
% pitch
Kpp = 0.33*Kup;
Tip = 0.5*Tup;
Tdp = 0.33*Tup;
sim('Ftest',200);
%% fuzzy control
n=100;
m=2;
FIS = newfis('FIS','mamdani','prod','probor','prod','sum');
FIS = addvar(FIS,'input','E',[-n n]);
FIS = addmf(FIS,'input',1,'Negative','trimf',[-m*n -n 0]);
FIS = addmf(FIS,'input',1,'Zero','trimf',[-n 0 n]);
FIS = addmf(FIS,'input',1,'Positive','trimf',[0 n m*n]);
FIS = addvar(FIS,'input','CE',[-n n]);
FIS = addmf(FIS,'input',2,'Negative','trimf',[-m*n -n 0]);
FIS = addmf(FIS,'input',2,'Zero','trimf',[-n 0 n]);
FIS = addmf(FIS,'input',2,'Positive','trimf',[0 n m*n]);
FIS = addvar(FIS,'output','u',[-m*n m*n]);
FIS = addmf(FIS,'output',1,'LargeNegative','trimf',[-m*n -m*n -m*n]);
FIS = addmf(FIS,'output',1,'SmallNegative','trimf',[-n -n -n]);
FIS = addmf(FIS,'output',1,'Zero','trimf',[0 0 0]);
FIS = addmf(FIS,'output',1,'SmallPositive','trimf',[n n n]);
FIS = addmf(FIS,'output',1,'LargePositive','trimf',[m*n m*n m*n]);
ruleList = [1 1 1 1 1;... % Rule 1
1 2 2 1 1;... % Rule 2
1 3 3 1 1;... % Rule 3
2 1 2 1 1;... % Rule 4
2 2 3 1 1;... % Rule 5
2 3 4 1 1;... % Rule 6
3 1 3 1 1;... % Rule 7
3 2 4 1 1;... % Rule 8
3 3 5 1 1]; % Rule 9
FIS = addrule(FIS,ruleList);
gensurf(FIS);
%%
FIS=readfis('FIS2');
sim('Ftest',40);