%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% measurements.m
% This program is a data acquisition program using the serial port in order
% to collect data from the inverted pendulum system.  
%
% Copyright 2005 by John Stang
%

close all;
clear all;
clear all;

% To construct a serial port object:
s1 = serial('COM1', 'BaudRate', 9600,'Parity', 'none', 'StopBits', 1,'FlowControl','none');
% To connect the serial port object to the serial port:
fopen(s1);
%record(s1,'on');
types = 6;

%%%%%%%%%%%%%%%%%%%%%%%%
% Initialize variables %
%%%%%%%%%%%%%%%%%%%%%%%%
freq = 100; % sampling frequency
t = 60; %time to sample
samples = t*freq; %number of samples per measurement
data = zeros(types,samples);
count = 1;
store = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Read in Serial Data From the Mega32 % 
% until Stop Command is Sent          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while (count <(samples+1)) 
    
   %fscanf(s1,'int',samples)
   data(:,count)  =  fread(s1,types);
   if (data(1,count) == 0) 
       store = count;
       count = samples+2;
   end
   count = count +1;
end

fclose(s1);
if (store~=0)
data = data(:,1:store-1);
end

[a b] = size(data);
time = (1:b)*1/freq;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Convert Readings into actual values %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
position = (data(1,:)*256+round(data(2,:)))* 0.4289/1000;
theta = ((data(3,:)*256+round(data(4,:)))-36)*(300/(1016-36))-150;
voltage = ((-1).^data(6,:)).*(5- data(5,:)*5/256);

%%%%%%%%%%%%%%%%%%%%%%%%%
% Plots of Measurements %
%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1);
plot(time,position);
title('Cart Position');
xlabel('time (sec)');
ylabel('Position  (m)');
figure(2);
plot(time,theta);
title('Pendulum Angle');
xlabel('time (sec)');
ylabel('Theta (degrees)');
figure(3);
plot(time,voltage);
title('Voltage Applied');
xlabel('time');
ylabel('Voltage');

position = position- position(1);
theta = theta- theta(1);
figure(1);
plot(time(1:4500),position(1:4500));
%plot(time, data(1,:));
%hold on;
%plot(time, data(2,:),'r');
title('Cart Position');
xlabel('time (sec)');
ylabel('Position  (m)');
figure(2);
plot(time(1:4500),theta(1:4500));
title('Pendulum Angle');
xlabel('time (sec)');
ylabel('Theta (degrees)');
figure(3);
plot(time(1:4500),voltage(1:4500));
title('Voltage Applied');
xlabel('time');
ylabel('Voltage');