From 7f855e0172ff08e772d10ab3b64bc43d3301523d Mon Sep 17 00:00:00 2001
From: ucart <ucart_groundstation@iastate.edu>
Date: Tue, 25 Apr 2017 19:23:31 -0500
Subject: [PATCH] changes to scripts and simpleplots

---
 controls/DataAnalysisTool/Tool/DataAnalysis.m |   8 +-
 controls/DataAnalysisTool/Tool/simplePlots.m  | 168 +++++++++++++++---
 groundStation/scripts/parameterize.sh         |  17 +-
 3 files changed, 160 insertions(+), 33 deletions(-)

diff --git a/controls/DataAnalysisTool/Tool/DataAnalysis.m b/controls/DataAnalysisTool/Tool/DataAnalysis.m
index a7b873173..b8cb1710e 100644
--- a/controls/DataAnalysisTool/Tool/DataAnalysis.m
+++ b/controls/DataAnalysisTool/Tool/DataAnalysis.m
@@ -51,9 +51,9 @@ backgnd     = [1 1 1];  % rgb array for background color of the plot
 
 
 %% DO NOT MODIFY %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fpath = '';
+fpath = '/local/ucart/Documents/MicroCART_17-18/groundStation/logs/';
 if(isempty(fname))
-    [fname, fpath] = uigetfile('.txt','Select log file');
+    [fname, fpath] = uigetfile('.txt','Select log file', fpath);
 end
 
 % storing file options in the structure
@@ -100,4 +100,6 @@ plot_data(expData, params.plotting);
 % main.params     = params;
 % main.expData    = expData;
 % clearvars -except main;
-% save main main;
\ No newline at end of file
+% save main main;
+
+clear plot;
\ No newline at end of file
diff --git a/controls/DataAnalysisTool/Tool/simplePlots.m b/controls/DataAnalysisTool/Tool/simplePlots.m
index a1087dfda..330ba82b4 100644
--- a/controls/DataAnalysisTool/Tool/simplePlots.m
+++ b/controls/DataAnalysisTool/Tool/simplePlots.m
@@ -97,7 +97,7 @@ all_motors = expData.Signal_Mixer_MOTOR_0.data + expData.Signal_Mixer_MOTOR_1.da
     expData.Signal_Mixer_MOTOR_2.data + expData.Signal_Mixer_MOTOR_3.data;
 ax1 = subplot(1, 2, 1);
 plot(expData.Time.data, all_motors ./ 4); hold on;
-plot(expData.Time.data, expData.RC_Throttle_Constant.data); hold on;
+%plot(expData.Time.data, expData.RC_Throttle_Constant.data); hold on;
 plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
 plot(expData.Time.data, expData.Roll_Rate_PID_Correction.data); hold on;
 plot(expData.Time.data, expData.Yaw_Rate_PID_Correction.data);
@@ -109,11 +109,11 @@ linkaxes([ax1, ax2], 'x');
 
 %%
 ax1 = subplot(1, 2, 1);
-plot(expData.Time.data, expData.Pitch_Constant.data .* (180 / pi)); hold on; grid minor
+%plot(expData.Time.data, expData.Pitch_Constant.data .* (180 / pi)); hold on; grid minor
 plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* (180 / pi));
 legend('imu', 'vrpn');
 ax2 = subplot(1, 2, 2);
-plot(expData.Time.data, expData.Roll_Constant.data .* (180 / pi)); hold on; grid minor
+%plot(expData.Time.data, expData.Roll_Constant.data .* (180 / pi)); hold on; grid minor
 plot(expData.Time.data, expData.VRPN_Roll_Constant.data .* (180 / pi));
 legend('imu', 'vrpn');
 linkaxes([ax1, ax2], 'x');
@@ -163,8 +163,8 @@ linkaxes([ax1, ax2, ax3, ax4], 'x');
 %% vel flow
 figure;
 ax2 = subplot(2,2,1);
-plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data - expData.RC_Pitch_Constant.data); hold on;
 plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data); hold on;
+%plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data); hold on;
 %plot(expData.Time.data, expData.X_Vel_Correction.data); hold on;
 %plot(expData.Time.data, [0; raw_derivative]);
 title('X velocity error');
@@ -187,6 +187,7 @@ title('pitch rate output');
 
 linkaxes([ax1, ax2, ax3, ax4], 'x');
 %%
+figure;
 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;
@@ -222,12 +223,12 @@ figure;
 plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
 plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
 
-angle = sqrt(expData.Roll_Constant.data.^2 + expData.VRPN_Pitch_Constant.data.^2);
-corrected = expData.Lidar_Constant.data .* cos(angle);
+%angle = sqrt(expData.Roll_Constant.data.^2 + expData.VRPN_Pitch_Constant.data.^2);
+%corrected = expData.Lidar_Constant.data .* cos(angle);
 %plot(expData.Time.data, corrected);
-legend('lidar', 'vrpn', 'angle corrected');
+legend('lidar', 'vrpn');
 
-linkaxes([ax1, ax2], 'x');
+%linkaxes([ax1, ax2], 'x');
 
 %% Sonar
 filtered_sonar = [];
@@ -246,33 +247,158 @@ plot(expData.Time.data, -expData.Flow_Distance_Constant.data + alt_offset); hold
 plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
 plot(expData.Time.data, -filtered_sonar + alt_offset);
 legend('sonar', 'vrpn', 'dumb filter');
-%%
+%% THE ONE
 figure;
 
-offsetX = -expData.OF_Integrate_X_Integrated.data(1) - expData.VRPN_X_Constant.data(1);
-offsetY = -expData.OF_Integrate_Y_Integrated.data(1) - expData.VRPN_Y_Constant.data(1);
+% offsetX = -expData.OF_Integrate_X_Integrated.data(1) - expData.VRPN_X_Constant.data(1);
+% offsetY = -expData.OF_Integrate_Y_Integrated.data(1) - expData.VRPN_Y_Constant.data(1);
+offsetX = 0;
+offsetY = 0;
 
-ax1 = subplot(2, 1, 1);
-plot(expData.Time.data, -expData.OF_Integrate_X_Integrated.data - offsetX); hold on;
+ax1 = subplot(3, 1, 1);
+plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data - offsetX); hold on; grid minor
 plot(expData.Time.data, expData.VRPN_X_Constant.data);
-legend('OF X Position', 'VRPN X Position');
+plot(expData.Time.data, expData.X_Setpoint_Constant.data);
+legend('OF X Position', 'VRPN X Position', 'X setpoint');
 xlabel('Time (s)');
 ylabel('Displacement (m)');
 hold off;
 
-ax2 = subplot(2, 1, 2);
-plot(expData.Time.data, -expData.OF_Integrate_Y_Integrated.data - offsetY); hold on;
+ax2 = subplot(3, 1, 2);
+plot(expData.Time.data, expData.OF_Integrate_Y_Integrated.data - offsetY); hold on; grid minor
 plot(expData.Time.data, expData.VRPN_Y_Constant.data);
-legend('OF Y Position', 'VRPN Y Position');
+plot(expData.Time.data, expData.Y_Setpoint_Constant.data);
+legend('OF Y Position', 'VRPN Y Position', 'Y setpoint');
+xlabel('Time (s)');
+ylabel('Displacement (m)');
+hold off;
+
+ax3 = subplot(3, 1, 3);
+plot(expData.Time.data, expData.Lidar_Constant.data); hold on; grid minor
+plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
+plot(expData.Time.data, expData.Alt_Setpoint_Constant.data);
+legend('Lidar Z Position', 'VRPN Z Position', 'Z setpoint');
 xlabel('Time (s)');
 ylabel('Displacement (m)');
 hold off;
 
-linkaxes([ax1, ax2]);
 
 
-%% Integarted gyro yaw
+linkaxes([ax1, ax2, ax3], 'x');
+%% Error Graphs
 figure;
-plot(expData.Time.data, cumtrapz(expData.gyro_z.data + 0.0088)); hold on;
-plot(expData.Time.data, expData.Yaw_Constant.data); hold on;
+ax1 = subplot(2,1,1);
+plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.OF_Integrate_X_Integrated.data);
+title('X error');
+
+ax2 = subplot(2,1,2);
+plot(expData.Time.data, expData.Y_Setpoint_Constant.data - expData.OF_Integrate_Y_Integrated.data);
+title('Y error');
+
+%% Integarted gyro0.55 yaw
+figure;
+gyro_yaw = 0.005 * cumtrapz(expData.gyro_z.data + 0.0073);
+plot(expData.Time.data, (180/pi) * gyro_yaw); hold on;  
+plot(expData.Time.data, expData.Yaw_Constant.data * 180/pi); hold on;
 legend('Integrated gyro z', 'actual yaw');
+ylabel('Yaw (degrees)');
+xlabel('Time (s)');
+
+%%
+figure;
+angleOffset = 0.62204 + gyro_yaw;
+
+FlowVelX = expData.Flow_Vel_X_Constant.data.*cos(angleOffset) - expData.Flow_Vel_Y_Constant.data.*sin(angleOffset);
+FlowVelY = expData.Flow_Vel_X_Constant.data.*sin(angleOffset) + expData.Flow_Vel_Y_Constant.data.*cos(angleOffset);
+
+fc = 10;
+FlowVelX = BiquadFilter(FlowVelX, 200, fc);
+FlowVelY = BiquadFilter(FlowVelY, 200, fc);
+
+flowX = zeros(1, length(expData.Time.data));
+
+driftX = 0;
+driftY = 0;
+
+flowX(1) = expData.VRPN_X_Constant.data(1);
+for n = 2:length(flowX)
+    flowX(n) = flowX(n-1) + 0.005*(FlowVelX(n) - driftX);
+end
+
+flowY = zeros(1, length(expData.Time.data));
+flowY(1) = expData.VRPN_Y_Constant.data(1);
+for n = 2:length(flowY)
+    flowY(n) = flowY(n-1) + 0.005*(FlowVelY(n) - driftY);
+end
+
+ax1 = subplot(2, 1, 1);
+plot(expData.Time.data, flowX); hold on;
+plot(expData.Time.data, expData.VRPN_X_Constant.data);
+%legend('OF Integrated X Position');
+legend('OF Integrated X', 'VRPN X');
+%legend('OF Integrated X Position', 'Approximate Max X Position (measured with tape measure)');
+xlabel('Time (s)');
+ylabel('Position (m)');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+plot(expData.Time.data, flowY); hold on;
+plot(expData.Time.data, expData.VRPN_Y_Constant.data);
+%legend('OF Integrated Y Position');
+legend('OF Integrated Y', 'VRPN Y');
+%legend('OF Integrated Y Position', 'Approximate Max Y Position (measured with tape measure)');
+xlabel('Time (s)');
+ylabel('Position (m)');
+hold off;
+
+linkaxes([ax1 ax2]);
+%%
+figure;
+ax1 = subplot(2, 1, 1);
+plot(expData.Time.data, expData.Flow_Quality_Constant.data);
+ax2 = subplot(2, 1, 2);
+plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
+plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
+linkaxes([ax1 ax2], 'x');
+
+%%
+figure;
+ax1 = subplot(3, 1, 1);
+plot(expData.Time.data, expData.mag_x.data);
+ax2 = subplot(3, 1, 2);
+plot(expData.Time.data, expData.mag_y.data);
+ax3 = subplot(3, 1, 3);
+plot(expData.Time.data, expData.mag_z.data);
+linkaxes([ax1 ax2 ax3], 'x');
+
+%% 
+figure;
+filtX = BiquadFilter(expData.mag_x.data+33.9844, 200, 1);
+filtY = BiquadFilter(expData.mag_y.data-40.4922, 200, 1);
+
+magYaw = atan2(-filtY, -filtX);
+gyroYaw = cumtrapz(expData.gyro_z.data - 0.008) * 0.005;
+
+ax1 = subplot(3, 1, 1);
+plot(expData.Time.data, magYaw*180/pi);
+ax2 = subplot(3, 1, 2);
+plot(expData.Time.data, gyroYaw*180/pi);
+ax3 = subplot(3, 1, 3);
+plot(expData.Time.data, expData.Mag_Yaw_Constant.data*180/pi);
+
+linkaxes([ax1 ax2 ax3], 'x');
+
+%% 
+mag = expData.mag_z.data;
+
+count = 0;
+for n = 2:length(mag)
+    if mag(n) == mag(n-1)
+        count = count + 1;
+        if count >= 10
+            disp(['Stall detected at index ' num2str(n)]);
+        end
+    else
+        count = 0;
+    end
+end
\ No newline at end of file
diff --git a/groundStation/scripts/parameterize.sh b/groundStation/scripts/parameterize.sh
index 591656810..9554cc5d7 100755
--- a/groundStation/scripts/parameterize.sh
+++ b/groundStation/scripts/parameterize.sh
@@ -1,10 +1,9 @@
-cd ..
-./addnode 0 "Zero"
-./setparam "zero" 0 0
-./setsource "signal mixer" "pitch" "zero" 0
-./setsource "signal mixer" "roll" "zero" 0
-./setsource "signal mixer" "yaw" "zero" 0
-./addnode 0 "PWM_VAL"
-./setparam "pwm_val" 0 100000
-./setsource "signal mixer" "throttle" "pwm_val" 0
+./setparam 'x pos pid' 0 -0.015
+./setparam 'x pos pid' 1 -0.0075
+./setparam 'x pos pid' 2 -0.08
+./setparam 'x pos pid' 3 0.94
 
+./setparam 'y pos pid' 0 0.015
+./setparam 'y pos pid' 1 0.0075
+./setparam 'y pos pid' 2 0.08
+./setparam 'y pos pid' 3 0.94
-- 
GitLab