-
Notifications
You must be signed in to change notification settings - Fork 0
/
plot_algo_zach.m
213 lines (184 loc) · 5.08 KB
/
plot_algo_zach.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
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
%% Housekeeping
addpath('Quaternions'); % include quatenrion library
% close('figure 1') % close all figures
clear; % clear all variables
clc; % clear the command terminal
%% Import data
data = get_data;
linaccx = data(:,11);
linaccy = data(:,12);
linaccz = data(:,13);
dt = 1/100;
acc = data(:,2:4);
gyr = data(:,5:7);
euler = data(:,8:10);
quat = data(:,17:20);
% linPos = data_crunching;
% linPos = linPos(:,9:11);
linPos = filter_posns;
pos_x = linPos(:,1);
pos_y = linPos(:,2);
pos_z = linPos(:,3);
hoop = make_hoop;
% old_pos_y = pos_y;
% old_pos_x = pos_x;
%% Filter Data
% pos_ind = find(pos_x>2,1); %Removes data from picking up ball
% % ind = min(find(linaccx(pos_ind:end)>8,1),find(linaccz(pos_ind:end)>8,1))+pos_ind; %Filter data based on g's (NEED TO TEST CONSISTENCY)
%
% % pos_x = pos_x(1:ind);
% % pos_y = pos_y(1:ind);
% % pos_z = pos_z(1:ind);
%
% % pos_y = old_pos_y + old_pos_x/12.3; %Correct misalignment: 69 --> 12.3164
% pos_y = old_pos_y + old_pos_x/5; %Correct misalignment: C
% % pos_y = old_pos_y + old_pos_x/-40; %Correct misalignment: D
%% Generate Plot (with "hoop" overlayed) --> NEED TO RE-ANIMATE (AND ADD DEPTH OF BASKET?), MOVE HOOP CODE TO ANOTHER FUNCTION???
bcx = 0; %ball center, x, inches
bcy = 0; %ball center, y, inches
bcz = 32.5; %ball center, z, inches
rcx = 90; %rim center, x, inches
rcy = 0; %rim center, y, inches
rcz = 76; %rim center, z, inches
rd = 14; %rim depth, inches
rw = 14; %rim width, inches
bbw = 34.5; %back board width, inches
bbh = 19; %back baord height, inches
drcbb = 3.25; %distance of rim center to back board, inches
% Side View
figure('units','normalized','outerposition',[0 0 1 1]);
subplot(1,2,1)
line([(rcx-bcx-rd/2)/12,(rcx-bcx+rd/2)/12],[(rcz-bcz)/12,(rcz-bcz)/12],'LineWidth',5); %hoop
line([(rcx-bcx+rd/2)/12,(rcx-bcx+rd/2)/12],[(rcz-bcz)/12,(rcz-bcz+bbh)/12],'LineWidth',5); %back board
hold on;
scatter(pos_x,pos_z,500,[1 .5 0])
title('Side View')
xlabel('X (ft)')
ylabel('Z (ft)')
xlim([-2 10])
ylim([-2 8])
% Top View
subplot(1,2,2)
scatter(-pos_y,pos_x,500,[1 .5 0])
hold on;
rectangle('Position', [-(rw/2)/12 (rcx-bcx-rd/2)/12 rw/12 rd/12],'LineWidth',5); %hoop
line([(-bbw/2)/12,+(bbw/2)/12],[(rcx-bcx+rd/2+drcbb)/12,(rcx-bcx+rd/2+drcbb)/12],'LineWidth',5); %back board
title('Top View')
xlabel('Y (ft)')
ylabel('X (ft)')
xlim([-6 6])
ylim([-2 10])
%% Old Code
% zc = z_end;
% theta = linspace(0,2*pi);
% c = r*cos(theta) + zc;
% d = r*sin(theta) + yc;
% %plot(c,d)
% SixDOFanimation(linPos, R, ...
% 'SamplePlotFreq', SamplePlotFreq, 'Trail', 'DotsOnly', ...
% 'Position', [9 39 1280 720], ...
% 'AxisLength', 0.1, 'ShowArrowHead', true, ...
% 'Xlabel', 'X (m)', 'Ylabel', 'Y (m)', 'Zlabel', 'Z (m)', 'ShowLegend', false, 'Title', 'Unfiltered',...
% 'CreateAVI', true, 'AVIfileNameEnum', false, 'AVIfps', ((1/dt) / SamplePlotFreq));
% scatter3(pos_x,pos_y,pos_z);
% title('Profile View')
% % axis([0,12,0,15,0,12]);
% xlabel('X (ft)')
% ylabel('Y (ft)')
% zlabel('Z (ft)')
% view([90 0])
% grid on
% hold on
% surf(X,Y,Z);
% hold off
% subplot(1,2,1)
% surf(X,Y,Z)
% view([90 0])
% hold on
% comet3(pos_x,pos_y,pos_z)
% hold off
% subplot(1,2,2)
% r = .667;
% theta = linspace(0,2*pi);
% a = r*cos(theta);
% b = r*sin(theta)+12;
% plot(a,b,'k')
% hold on
% title('Top View')
% xlabel('X (ft)')
% ylabel('Y (ft)')
% xlim([-1 1])
% ylim([0 15])
% grid on
% hold on
% comet(pos_x,pos_y,.5)
% hold off
% HOOP
% X = hoop(1:2,1:21);
% Y = hoop(1:2,22:42);
% Z = hoop(1:2,43:63);
% x_end = hoop(1,21);
% y_end = hoop(1,42);
% z_end = hoop(1,63);
% r = .875/2;
% theta = linspace(0,2*pi);
% a = r*cos(theta);
% b = r*sin(theta)+12;
% figure;
% plot(pos_x,pos_y);
% xlabel('X (ft)')
% ylabel('Y (ft)')
% figure;
% plot(pos_x,pos_z);
% xlabel('X (ft)')
% ylabel('Z (ft)')
% figure;
% plot(pos_y,pos_z);
% xlabel('Y (ft)')
% ylabel('Z (ft)')
% figure(2)
% subplot(1,2,1)
% scatter(pos_y,pos_z)
% title('Side View')
% xlabel('Y (ft)')
% ylabel('Z (ft)')
%
% subplot(1,2,2)
% scatter(pos_x,pos_y);
% title('Top View')
% xlabel('X (ft)')
% ylabel('Y (ft)')
% figure('units','normalized','outerposition',[0 0 1 1]);
% scatter3(pos_x,pos_y,pos_z,[],[1 .5 0]);
% for i = 1:length(pos_x
%
% % PROFILE VIEW
%
% hold on
% scatter3(pos_x(i),pos_y(i),pos_z(i),[],[1 .5 0])
% pause(.001)
%
% % % TOP VIEW
% % subplot(1,2,2)
% % hold on
% % scatter(pos_x(i),pos_y(i),100,[1 .5 0])
%
% end
%rectangle('Position', [-.4375 12 .875 1.167]);
% Animate trajectory
% for i = 1:length(pos_x)
%
% % PROFILE VIEW
% subplot(1,2,1)
% hold on
% scatter3(pos_x(i),pos_y(i),pos_z(i),[],[1 .5 0])
% pause(.001)
%
% % TOP VIEW
% subplot(1,2,2)
% hold on
% scatter(pos_x(i),pos_y(i),100,[1 .5 0])
%
% end
% scatter3(pos_x,pos_y,pos_z)
%% End of script