Something went wrong on our end
simplePlots.m 8.99 KiB
%%
plot(Time, Cam_Meas_Roll*(180/pi)); hold on;
plot(Time, Quad_Meas_Roll*(180/pi));
%%
plot(Time, pitch_velocity*(180/pi)); hold on;
plot(Time, roll_velocity*(180/pi));
plot(Time, yaw_velocity*(180/pi));
%%
figure;
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');
%%
figure;
plot(time, (VRPNRollConstant + pi)*(180/pi)); hold on;
plot(time, RollConstant*(180/pi));
legend('Camera Roll', 'Quad Roll');
xlabel('seconds'); ylabel('degrees');
%%
%plot(Time, X_setpoint); hold on;
markerSize = 3;
ax1 = subplot(2,1,1);
plot(time, (XSetpointConstant - VRPNXConstant), '-o', 'MarkerSize', markerSize); hold on;
plot(time, -XposPIDCorrection * (180/pi), '-o', 'MarkerSize', markerSize); hold off;
legend('X Error', 'X PID output');
ax2 = subplot(2,1,2);
plot(time, (180/pi)*PitchConstant, '-o', 'MarkerSize', markerSize); hold on;
plot(time, (180/pi)*PitchPIDCorrection, '-o', 'MarkerSize', markerSize);
legend('Pitch Error', 'Pitch PID output');
linkaxes([ax1, ax2], 'x');
%%
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.VRPN_X_Constant.data);
title('X error');
ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_pos_PID_Correction.data);
title('x 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');
%% X pos controller flow
figure;
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.OF_Integrate_X_Integrated.data);
title('X error');
ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_pos_PID_Correction.data);
title('x 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);
legend('output', 'Pitch rate');
title('pitch rate output');
linkaxes([ax1, ax2, ax3, ax4], 'x');
%%
plot(time, 1044.26 .* (PitchPIDCorrection - gyro_y));hold on;
%plot(time, PitchRatePIDCorrection);
%%
ax2 = subplot(2, 1, 1);
plot(time, YawConstant);
ax1 = subplot(2, 1, 2);
plot(time, gyro_z); hold on;
plot(time, YawRatePIDCorrection);
linkaxes([ax1, ax2], 'x');
%%
all_motors = expData.Signal_Mixer_MOTOR_0.data + expData.Signal_Mixer_MOTOR_1.data + ...
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.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);
legend('average motors', 'throttle', 'pitch', 'roll', 'yaw');
ax2 = subplot(1, 2, 2);
plot(expData.Time.data, -expData.VRPN_Alt_Constant.data);
legend('Z, meters');
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.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.VRPN_Roll_Constant.data .* (180 / pi));
legend('imu', 'vrpn');
linkaxes([ax1, ax2], 'x');
%%
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);
ax2 = subplot(3, 1, 2);
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');
%%
figure;
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');
%% vel flow
figure;
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data - expData.X_pos_PID_Correction.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');
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');
%% Sonar
filtered_sonar = [];
last_sonar = expData.Flow_Distance_Constant.data(1);
for i = [1 : length(expData.Flow_Distance_Constant.data)]
this_sonar = expData.Flow_Distance_Constant.data(i);
if (abs(this_sonar - last_sonar) < 0.4)
filtered_sonar(i) = this_sonar;
last_sonar = this_sonar;
else
filtered_sonar(i) = last_sonar;
end
end
alt_offset = -0.04;
plot(expData.Time.data, -expData.Flow_Distance_Constant.data + alt_offset); hold on;
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
plot(expData.Time.data, -filtered_sonar + alt_offset);
legend('sonar', 'vrpn', 'dumb filter');
%%
figure;
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data); hold on;
plot(expData.Time.data, expData.VRPN_X_Constant.data);
legend('OF X Position', 'VRPN X Position');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;
ax2 = subplot(2, 1, 2);
plot(expData.Time.data, expData.OF_Integrate_Y_Integrated.data); hold on;
plot(expData.Time.data, expData.VRPN_Y_Constant.data);
legend('OF Y Position', 'VRPN Y Position');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;
linkaxes([ax1, ax2]);
%% Integarted gyro yaw
figure;
plot(expData.Time.data, cumtrapz(expData.gyro_z.data + 0.0088)); hold on;
plot(expData.Time.data, expData.Yaw_Constant.data); hold on;
legend('Integrated gyro z', 'actual yaw');