% TOGAC33_AMBF_MATLAB2ROSfreqtestFigureMaker.m
% This file loads in the data from the frequency speed tests between matlab
% and ROS when sending commands to ambf from matlab. Since significant
% delay was observed between communication, it was imporant to identify a
% method to remove the delay. The data collected here was tested with a
% virtual machine of linux 20.04 running Ros noetic with ambf and matlab in
% the vm. The speed issued was identified as a slow computer and the
% simulations could only be performed at a maximum of 200Hz, but higher
% speeds may be desired. Use a more powerful computer to avoid these
% issues. Although it should be noted that the Galen runs at 200Hz
% currently.
% Created by Brevin Banks
% Modified April 1 2023

% Load the data from tests with python and ambf
python_freq_data_1000 = load('python_dataat1000nograv.mat')
python_freq_data_1000_grav = load('python_dataat1000.mat')
python_freq_data_200 = load('python_dataat1000nograv.mat')
python_freq_data_200_grav = load('python_dataat1000.mat')

% Load the data from tests with matlab and ambf
Vin100hzto1hzChirp = load('Vin100hzto1hzChirp.mat')
VinStep0_05 = load('VinStep0_05.mat')
VinStep0_05Grav = load('VinStep0_05Grav.mat')

Vout100hzto1hzChirp = load('Vout100hzto1hzChirp.mat')
VoutStep0_05 = load('VoutStep0_05.mat')
VoutStep0_05Grav = load('VoutStep0_05Grav.mat')
VoutStep0_05_Nograv = load('VoutStep0_05Nograv.mat')

Vin100hzto1hzChirp.Vin100hzto1hz;
VinStep0_05.rv;
VinStep0_05Grav.rv;
Vout100hzto1hzChirp.Vout100hzto1hz;
VoutStep0_05.cv;
VoutStep0_05Grav.cv;
VoutStep0_05_Nograv.cv;


%%
clc
if ishandle(1)
    close 1
end

figure('name','Matlab at 200Hz python at 200Hz no gravity')
% x = linspace(0,7.46,length(VinStep0_05.rv));
x = linspace(0,current_sim_time,length(rv));
subplot(2,1,1)
hold on
% plot(x,VinStep0_05.rv(1,:))
% plot(x,VoutStep0_05_Nograv.cv(1,:))
plot(x,rv(1,:))
plot(x,cv(1,:))
legend('Vref','Vout')
ylabel('Velocity (m/s)')
title('Matlab at 200Hz no gravity')

xp = linspace(0,python_freq_data_200.t(end),length(python_freq_data_200.cv));
subplot(2,1,2)
hold on
% plot(xp,python_freq_data_200.rv(1,:))
plot(x,rv(1,:))
plot(xp,python_freq_data_200.cv(1,:))
legend('Vref','Vout')
xlim([0 current_sim_time])
xlabel('time')
ylabel('Velocity (m/s)')
ylabel('Velocity (m/s)')
title('Python at 200Hz no gravity')


%%
clc
if ishandle(2)
    close 2
end

figure('name','Matlab at 1000Hz python at 1000Hz with gravity')
% x = linspace(0,7.46,length(VinStep0_05.rv));
x = linspace(0,current_sim_time,length(rv));
subplot(2,1,1)
hold on
% plot(x,VinStep0_05.rv(1,:))
% plot(x,VoutStep0_05_Nograv.cv(1,:))
plot(x,rv(1,:))
plot(x,cv(1,:))
legend('Vref','Vout')
ylabel('Velocity (m/s)')
title('Matlab at 1000Hz with gravity')

xp = linspace(0,python_freq_data_1000.t(end),length(python_freq_data_1000_grav.cv));
subplot(2,1,2)
hold on
plot(x,rv(1,:))
plot(xp,python_freq_data_1000_grav.cv(1,:))
legend('Vref','Vout')
xlim([0 current_sim_time])
xlabel('time')
ylabel('Velocity (m/s)')
ylabel('Velocity (m/s)')
title('Python at 1000Hz with gravity')
legend('Vref','Vout')


figure(3)
x = linspace(0,current_sim_time,length(rv));
hold on
plot(x,rv(1,:))
plot(x,cv(1,:))
legend('Vref','Vout')
ylabel('Velocity (m/s)')
title('Matlab at 200Hz no gravity. Chirp response')