-
Notifications
You must be signed in to change notification settings - Fork 5
/
f_energyMR.m
59 lines (49 loc) · 1.23 KB
/
f_energyMR.m
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
%objetive function
function [energy, D, gamma_return] = f_energyMR(M, dx, curve_radius, v, w, gamma_w, b0, b1 ,b2)
%gamma_w = pi/4;
%u = 10;
%r = 15;
%t0 = 0;
%t1 = 4;
%% Calculate path
%disp('Calculate path');
[Path, D] = getPathMR(M, dx, curve_radius);
% D = [lb lf ll lr];
lb = D(1);
lf = D(2);
ll = D(3);
lr = D(4);
%ltotal = lf + lb +ll+ lr;
%ltotal = lf + lb; %remove
%% Calculate energy
%disp('Estimate energy');
power_mr = @(v, w, angle, b0, b1, b2) b0 + b1 * abs(v - w*cos(angle)) + b2 * abs(w * sin(angle));
% fordward energy
theta = pi/2;
t = lf/v;
gamma = gamma_w-theta;
gamma_return = gamma;
p = power_mr(v,w,gamma,b0,b1,b2);
ef = p * t;
%backward energy
theta = 3*pi/2;
t = lb/v;
gamma = gamma_w-theta;
p = power_mr(v, w, gamma, b0, b1, b2);
eb = p * t;
%right energy
theta = 0;
t = lr/v;
gamma = gamma_w-theta;
p = power_mr(v,w,gamma,b0,b1,b2);
er = p * t;
%left energy
theta = 0;
t = ll/v;
gamma = gamma_w-theta;
p = power_mr(v,w,gamma,b0,b1,b2);
el = p * t;
energy = ef + eb + el + er;
%pause
%exit 0
end