%-------------------------------------------------------------------------
%-------------------------------------------------------------------------
% COURSE:   RBE502 ROBOT CONTROL 
%
% ASSIGNMENT:TERM PROJECT 
%
% AUTHORS:  KYLE CANTRELL (KJCANTRELL@WPI.EDU)
%           CRAIG MILLER  (CDMILLER@WPI.EDU)
%           JORDAN NELSON (JVNELSON@WPI.EDU)
%
% DATE:     04/27/2019
%-------------------------------------------------------------------------
% TITLE: Open_Manipulator_ROS_Data 1-2 Timestep
%-------------------------------------------------------------------------

clc; clear all; close all;
load('jointstatedata.mat');
load('odomdata.mat');

%Joint Positions
figure('Name','ROS Joint Theta Values Vs. Time 12');
plot(jointstatedata.timensec(26554:37198), ...
    jointstatedata.joint1position(26554:37198), ...
    jointstatedata.timensec(26554:37198), ...
    jointstatedata.joint2position(26554:37198), ...
    jointstatedata.timensec(26554:37198), ...
    jointstatedata.joint3position(26554:37198), ...
    jointstatedata.timensec(26554:37198), ...
    jointstatedata.joint4position(26554:37198));
hold on;
xlabel('Time in Seconds');
ylabel('Radians');
legend('Theta 1', 'Theta 2', 'Theta 3', 'Theta 4','Location','Best');
title('ROS Joint Theta Values Vs. Time 12');
grid on;

%Joint Velocities
figure('Name','ROS Joint Velocities Vs. Time 12');
plot(jointstatedata.timensec(26554:37198), ...
    jointstatedata.joint1velocity(26554:37198), ...
    jointstatedata.timensec(26554:37198), ...
    jointstatedata.joint2velocity(26554:37198), ...
    jointstatedata.timensec(26554:37198), ...
    jointstatedata.joint3velocity(26554:37198), ...
    jointstatedata.timensec(26554:37198), ...
    jointstatedata.joint4velocity(26554:37198));
hold on;
xlabel('Time in Seconds');
ylabel('Radians/Sec');
legend('Theta 1', 'Theta 2', 'Theta 3', 'Theta 4','Location','Best');
title('ROS Joint Velocities Vs. Time 12');
grid on;

% %Joint Torques
% figure('Name','ROS Joint Torques Vs. Time 12');
% plot(jointstatedata.timensec(26554:37198), ...
%     jointstatedata.joint1effort(26554:37198), '-',...
%     jointstatedata.timensec(26554:37198), ...
%     jointstatedata.joint2effort(26554:37198), '-', ...
%     jointstatedata.timensec(26554:37198), ...
%     jointstatedata.joint3effort(26554:37198), '-', ...
%     jointstatedata.timensec(26554:37198), ...
%     jointstatedata.joint4effort(26554:37198), '-');
% hold on;
% xlabel('Time in Seconds');
% ylabel('Torque (Nm)');
% legend('Theta 1', 'Theta 2', 'Theta 3', 'Theta 4','Location','Best');
% title('ROS Joint Torques Vs. Time 12');
% grid on;

%Filtered Joint Torques
figure('Name','ROS Filtered Joint Torques Vs. Time 12');
plot(jointstatedata.timensec(26554:37198), ...
    lowpass(jointstatedata.joint1effort(26554:37198),0.8), '-',...
    jointstatedata.timensec(26554:37198), ...
    lowpass(jointstatedata.joint2effort(26554:37198),0.8), '-', ...
    jointstatedata.timensec(26554:37198), ...
    lowpass(jointstatedata.joint3effort(26554:37198),0.8), '-', ...
    jointstatedata.timensec(26554:37198), ...
    lowpass(jointstatedata.joint4effort(26554:37198),0.8), '-');
hold on;
xlabel('Time in Seconds');
ylabel('Torque (Nm)');
legend('Theta 1', 'Theta 2', 'Theta 3', 'Theta 4','Location','Best');
title('ROS Filtered Joint Torques Vs. Time 12');
grid on;

% figure('Name','Mobile Robot Position vs. Time');
% plot(odomdata.timensec(1:897), ...
%      odomdata.x(1:897), ...
%      odomdata.timensec(1:897), ...
%      odomdata.y(1:897), ...
%      odomdata.timensec(1:897), ...
%      odomdata.z(1:897));
% 
% hold on;
% xlabel('Time in Seconds');
% ylabel('Meters');
% legend('X', 'Y', 'Z','Location','Best');
% title('Mobile Robot Position vs. Time');
% grid on;

% figure('Name','Mobile Robot Position (Overhead) Vs. Time');
% plot(odomdata.x(1:897), ...
%      odomdata.y(1:897));
% hold on;
% xlabel('X (m)');
% ylabel('Y (m)');
% title('Mobile Robot Position (Overhead) Vs. Time');
% grid on;

% figure('Name','Mobile Robot Linear Velocity Vs. Time');
% plot(odomdata.timensec(1:897), ...
%      odomdata.Xvelocity(1:897), ...
%      odomdata.timensec(1:897), ...
%      odomdata.Yvelocity(1:897), ...
%      odomdata.timensec(1:897), ...
%      odomdata.Zvelocity(1:897));
% hold on;
% xlabel('Time in Seconds');
% ylabel('M/s');
% legend('X', 'Y', 'Z','Location','Best');
% title('Mobile Robot Linear Velocity Vs. Time');
% grid on;

% figure('Name','Mobile Robot Angular Velocity Vs. Time');
% plot(odomdata.timensec(1:897), ...
%      odomdata.Xvelorientation(1:897), ...
%      odomdata.timensec(1:897), ...
%      odomdata.Yvelorientation(1:897), ...
%      odomdata.timensec(1:897), ...
%      odomdata.Zvelorientation(1:897));
% hold on;
% xlabel('Time in Seconds');
% ylabel('Radians/sec');
% legend('X', 'Y', 'Z','Location','Best');
% title('Mobile Robot Angular Velocity Vs. Time');
% grid on;


