bme411/mainScript.asv at master · jgeurten/bme411 · GitHub

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
% Main Script

% Initial launch conditions
% v0 = launch_params(1);      % initial speed [m/s]
%
% elev = launch_params(2);    % angle of elevation, theta in diagram
%
% wz = launch_params(3);      % back spin [rad/s]
% height = launch_params(4);  % height of release above ground [m]

close all;
setGlobalVars();

angles = 0:10:60;  %deg
backspins = 0:5:30; %rad/s
speeds = 0:4:20;

%lLot distances as a function of angle
figure, hold on
for i = 1:length(angles)
    launch_params = [20, angles(i), 10, 2];
    [x, final,t] = simBallTrajectory(launch_params);
    dispName = [num2str(angles(i)), ' deg'];
    plot(x(1:final,3),x(1:final,4), 'LineWidth', 3,'DisplayName', dispName );
%     plot(t(1:final), x(1:final, 4), 'LineWidth', 3,'DisplayName', dispName);
    legend('-DynamicLegend');
end
hold off
legend('show')
xlabel('Ball Distance [m]');
ylabel('Ball Height [m]');
title('Ball Trajectory vs Launch Angle')
saveas(gcf, 'Trajectory_vs_Angle.png');

figure, hold on
for i = 1:length(backspins)
    launch_params = [20, 20, backspins(i), 2];
    [x, final,t] = simBallTrajectory(launch_params);
    dispName = [num2str(backspins(i)), ' rad/s'];
    plot(x(1:final,3),x(1:final,4), 'LineWidth', 3,'DisplayName', dispName );
%     plot(t(1:final), x(1:final, 4), 'LineWidth', 3,'DisplayName', dispName);
    legend('-DynamicLegend');
end
hold off
legend('show')
xlabel('Ball Distance [m]');
ylabel('Ball Height [m]');
title('Ball Trajectory vs Launch Spn')
saveas(gcf, 'Trajectory_vs_Spin.png');


figure, hold on
for i = 1:length(speeds)
    launch_params = [speeds(i), 20, 10, 2];
    [x, final,t] = simBallTrajectory(launch_params);
    dispName = [num2str(speeds(i)), ' m/s'];
    plot(x(1:final,3),x(1:final,4), 'LineWidth', 3,'DisplayName', dispName );
%     plot(t(1:final), x(1:final, 4), 'LineWidth', 3,'DisplayName', dispName);
    legend('-DynamicLegend');
end
hold off
legend('show')
xlabel('Ball Distance [m]');
ylabel('Ball Height [m]');
title('Ball Trajectory vs Launch Speed')
saveas(gcf, 'Trajectory_vs_velocity.png');

%%

heights = [];
vels = [];
angles = 0:60;

for i = 1:length(angles)
    [vel, height] = calcVelocity(angles(i));
    heights = [heights, height];
    vels = [vels; vel];
end


figure, scatter(angles, heights);
xlabel('Angle (deg)');
ylabel('Launch Height (m)');
title('Launch Height versus Launch Angle');
saveas(gcf, 'height_vs_angle.png')

figure, scatter(angles, vels);
xlabel('Angle (deg)');
ylabel('Launch Speed (m/s)');
title('Launch Speed versus Launch Angle');
saveas(gcf, 'speed_vs_angle.png')

%% Optimization part of the code

%[ vel (m/s), angle (deg), backspin (rad/s)];
lBounds = [0, 0, 0];
uBounds = [20, 60, 10*pi];
initConds = [10, 35, 5];
options = optimoptions('fmincon','Display','iter');
nonlcon = @unitdisk;
[optimalLaunchConds, distance] = fmincon(@optimizeLaunchConds, initConds,[],[],[],[],lBounds, uBounds, [], options)

optimalVel = optimalLaunchConds(1);
optimalAngle = optimalLaunchConds(2);
optimalSpin = optimalLaunchConds(3);

angles = [round(optimalAngle)-5:2: round(optimalAngle)+5];
spins = [round(optimalSpin)-2:round(optimalSpin)+3];

figure, hold on
for i = 1:length(angles)

    [vel, height] = calcVelocity(angles(i));
    launchConds = [vel, angles(i), spins(i), height];
    [x, final,t] = simBallTrajectory(launchConds);
    dispName = [num2str(angles(i)), ' deg, ',num2str(spins(i)), ' rad/s' ];
    plot(x(1:final,3),x(1:final,4), 'LineWidth', 3,'DisplayName', dispName );
    legend('-DynamicLegend');
end


%% Plot distance as a function of angle, backspin


angles = 0:60;
spins = 0:10*pi;
%distances = zeros(length(angles)*length(spins),1);
count = 1;


figure, hold on
for i = 1:length(angles)
    for j = 1:length(spins)
        distance = optimizeLaunchConds([-1 , angles(i), spins(j)]);
         %count = count + 1;
         scatter3(angles(i), spins(j), distance);
         xlabel('Angle (deg)');
         ylabel('Spin (rad/s)');
         zlabel('Distance (m)');
    end
end


stop

hold off
legend('show')
xlabel('Ball Distance [m]');
ylabel('Ball Height [m]');
zlabel('Z [m]');
title('Ball Trajectory')