From 614fd067e2a220390a44ca57a0946f3b902be4d5 Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Wed, 12 Apr 2017 16:29:19 -0500
Subject: [PATCH] Updated simplePlots

---
 controls/DataAnalysisTool/Tool/simplePlots.m | 87 ++++++++++++++++++--
 1 file changed, 79 insertions(+), 8 deletions(-)

diff --git a/controls/DataAnalysisTool/Tool/simplePlots.m b/controls/DataAnalysisTool/Tool/simplePlots.m
index dfe97b5ae..57d029fc2 100644
--- a/controls/DataAnalysisTool/Tool/simplePlots.m
+++ b/controls/DataAnalysisTool/Tool/simplePlots.m
@@ -9,8 +9,8 @@ plot(Time, yaw_velocity*(180/pi));
 
 %% 
 figure;
-plot(time, VRPNPitchConstant*(180/pi)); hold on;
-plot(time, PitchConstant*(180/pi));
+plot(expData.Time.data, expData.VRPN_Pitch_Constant.data*(180/pi)); hold on;
+plot(expData.Time.data, expData.Pitch_trim_add_Sum.data*(180/pi) + 2.2);
 legend('Camera Pitch', 'Quad Pitch');
 xlabel('seconds'); ylabel('degrees');
 
@@ -49,12 +49,12 @@ ax3 = subplot(2,2,3);
 plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
 plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
 title('pitch output');
-legend('output', 'Pitch');
+legend('output', 'Pitch x10');
 
 ax4 = subplot(2,2,4);
 plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
-plot(expData.Time.data, expData.gyro_y.data .* 1044.26);
-legend('output', 'Pitch rate');
+plot(expData.Time.data, expData.gyro_y.data .* 100000);
+legend('output', 'Pitch rate x100000');
 title('pitch rate output');
 
 linkaxes([ax1, ax2, ax3, ax4], 'x');
@@ -96,9 +96,13 @@ legend('imu', 'vrpn');
 linkaxes([ax1, ax2], 'x');
 
 %%
-vrpn_roll_d = diff(expData.VRPN_Roll_Constant.data) / 0.005;
-plot(expData.Time.data, [0; vrpn_roll_d] .* (180 / pi)); hold on; grid minor;
-plot(expData.Time.data, expData.gyro_x.data .* (180 / pi));
+ax1 = subplot(3, 1, 1);
+plot(expData.Time.data, expData.accel_x.data);
+ax2 = subplot(3, 1, 2);
+plot(expData.Time.data, expData.accel_y.data);
+ax3 = subplot(3, 1, 3);
+plot(expData.Time.data, expData.accel_z.data);
+linkaxes([ax1, ax2, ax3], 'x');
 %%
 ax1 = subplot(3, 1, 1);
 plot(expData.Time.data, expData.gyro_x.data);
@@ -107,3 +111,70 @@ plot(expData.Time.data, expData.gyro_y.data);
 ax3 = subplot(3, 1, 3);
 plot(expData.Time.data, expData.gyro_z.data);
 linkaxes([ax1, ax2, ax3], 'x');
+%%
+ax2 = subplot(2,2,1);
+raw_derivative = -diff(expData.VRPN_X_Constant.data) / 0.04;
+plot(expData.Time.data, expData.X_Vel_Correction.data - (expData.RC_Pitch_Constant.data * 5)); hold on;
+%plot(expData.Time.data, expData.X_Vel_Correction.data); hold on;
+%plot(expData.Time.data, [0; raw_derivative]);
+title('X velocity error');
+
+ax1 = subplot(2,2,2);
+plot(expData.Time.data, expData.X_Vel_PID_Correction.data);
+title('x vel output');
+
+ax3 = subplot(2,2,3);
+plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
+plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
+title('pitch output');
+legend('output', 'Pitch x10');
+
+ax4 = subplot(2,2,4);
+plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
+plot(expData.Time.data, expData.gyro_y.data .* 100000);
+legend('output', 'Pitch rate x100000');
+title('pitch rate output');
+
+linkaxes([ax1, ax2, ax3, ax4], 'x');
+
+%%
+ax1 = subplot(2, 1, 1);
+plot(expData.Time.data, expData.Alt_Setpoint_Constant.data - expData.VRPN_Alt_Constant.data); hold on;
+plot(expData.Time.data, expData.Alt_Setpoint_Constant.data); hold on;
+plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
+legend('z error', 'z setpoint', 'z position');
+xlabel('time (s)');
+ylabel('meters');
+
+ax2 = subplot(2, 1, 2);
+plot(expData.Time.data, expData.Altitude_PID_Correction.data);
+linkaxes([ax1, ax2], 'x');
+legend('z PID output');
+xlabel('time (s)');
+ylabel('1e-8 seconds');
+
+
+%%
+ax1 = subplot(3, 1, 1);
+plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.VRPN_X_Constant.data);
+title('X error');
+
+ax2 = subplot(3, 1, 2);
+plot(expData.Time.data, expData.Y_Setpoint_Constant.data - expData.VRPN_Y_Constant.data);
+title('Y error');
+
+ax3 = subplot(3, 1, 3);
+plot(expData.Time.data, expData.Alt_Setpoint_Constant.data - expData.VRPN_Alt_Constant.data);
+title('Z error');
+linkaxes([ax1, ax2, ax3], 'x');
+%%
+ax1 = subplot(2, 1, 1);
+plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
+plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
+legend('lidar', 'vrpn');
+
+ax2 = subplot(2, 1, 2);
+plot(expData.Time.data, expData.Altitude_PID_Correction.data);
+
+linkaxes([ax1, ax2], 'x');
+%%
-- 
GitLab