%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% alpha_calc.m
% This code is used to generate the three parameters needed to charaterize
% the motor and friction in the inverted pendulum system designed for a
% Master's of Engineering Project.
%
% The files loaded in below are saved workspaces from experimental runs using
% the data acquisition program, measurements.m.  From these workspaces,
% measurements are then referenced to perform the necessary calculations.
%
%Copyright 2005 by John Stang
%

clc;
clear all;
close all;

%%%%%%%%%%%%%%%%
% Known Values %
%%%%%%%%%%%%%%%%
% This variable actually incorporates the necessary conversion from the
% poteniometer reading into the correct displacement
sprocket = .0023316;  
% Sampling Frequency
freq = 100;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculations for the barely moving case %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load bmleft1;
vleft(1) = voltage(1);
load bmleft2;
vleft(2) = voltage(1);
load bmleft3;
vleft(3) = voltage(1);
load bmleft4;
vleft(4) = voltage(1);
load bmright1;
vright(1) = voltage(1);
load bmright2;
vright(2) = voltage(1);
load bmright3;
vright(3) = voltage(1);
load bmright4;
vright(4) = voltage(1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Final average voltages for each direction %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
vleftavg = mean(vleft);
vrightavg = mean(vright);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculations for the terminal velocity experiment %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load tv2goodleft;
voltage = voltage*5/256;
%voltage = ((-1).^data(6,:)).*(5- data(5,:)*5/256);
voltleft(1) = voltage(1);
[a b] = size(data);
time = (1:b)*1/freq;
position = (data(1,:)*256+data(2,:))*sprocket;
subplot(3,2,1)
plot(time,position);
vl(1) = mean(diff(position(30:end)))/.01;
load tv3goodleft;
voltage = voltage*5/256;
%voltage = ((-1).^data(6,:)).*(5- data(5,:)*5/256);
voltleft(2) = voltage(1);
[a b] = size(data);
time = (1:b)*1/freq;
position = (data(1,:)*256+data(2,:))*sprocket;
subplot(3,2,3)
plot(time,position);
vl(2) = mean(diff(position(30:end)))/.01;
load tv1goodleft;
voltage = voltage*5/256;
%voltage = ((-1).^data(6,:)).*(5- data(5,:)*5/256);
voltleft(3) = voltage(1);
[a b] = size(data);
time = (1:b)*1/freq;
position = (data(1,:)*256+data(2,:))*sprocket;
subplot(3,2,5)
plot(time,position);
vl(3) = mean(diff(position(30:end)))/.01;
load tv2goodright;
voltage = voltage*5/256;
%voltage = ((-1).^data(6,:)).*(5- data(5,:)*5/256);
voltright(1) = voltage(1);
[a b] = size(data);
time = (1:b)*1/freq;
position = (data(1,:)*256+data(2,:))*sprocket;
subplot(3,2,2)
plot(time,position);
vr(1) = mean(diff(position(30:end)))/.01;
load tv3goodright;
voltage = voltage*5/256;
%voltage = ((-1).^data(6,:)).*(5- data(5,:)*5/256);
voltright(2) = voltage(1);
[a b] = size(data);
time = (1:b)*1/freq;
position = (data(1,:)*256+data(2,:))*sprocket;
subplot(3,2,4)
plot(time,position);
vr(2) = mean(diff(position(30:end)))/.01;
load tv1goodright;
voltage = voltage*5/256;
%voltage = ((-1).^data(6,:)).*(5- data(5,:)*5/256);
voltright(3) = voltage(1);
[a b] = size(data);
time = (1:b)*1/freq;
position = (data(1,:)*256+data(2,:))*sprocket;
subplot(3,2,6)
plot(time,position);
vr(3) = mean(diff(position(30:end)))/.01;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculations for the sinusiodal excitation experiment %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
load sin5gooda;
voltage = voltage*5/256;
[a b] = size(data);
time = (1:b)*1/freq;
position = (data(1,:)*256+data(2,:))*sprocket;
subplot(3,2,1)
a = 315;%1;
b = 3797;%length(position);
voltage = voltage(a:b);
position=detrend(position);
position=(position(a:b));
time = time(a:b);
avgvolt(1) = sqrt(2*mean(voltage.*voltage));
avgpos(1) = sqrt(2*mean(position.*position));
cosphi(1) = 2*mean(voltage.*position)/avgvolt(1)/avgpos(1);
sinphi(1) = sin(acos(cosphi(1)));
plot(time,position);

load sin5goodb;
[a b] = size(data);
time = (1:b)*1/freq;
a = 315;%1;
b = 3797;%length(position);
voltage = voltage(a:b);
position = (data(1,:)*256+data(2,:))*sprocket;
position=detrend(position);
position=(position(a:b));
time = time(a:b);
avgvolt(2) = sqrt(2*mean(voltage.*voltage));
avgpos(2) = sqrt(2*mean(position.*position));
cosphi(2) = 2*mean(voltage.*position)/avgvolt(2)/avgpos(2);
sinphi(2) = sin(acos(cosphi(2)));
subplot(3,2,2)
plot(time,position);

load sin10gooda;;
[a b] = size(data);
time = (1:b)*1/freq;
a = 670;%1;
b = 3844;%length(position);
position = (data(1,:)*256+data(2,:))*sprocket;
voltage = voltage(a:b);
position=detrend(position);
position=(position(a:b));
time = time(a:b);
avgvolt(3) = sqrt(2*mean(voltage.*voltage));
avgpos(3) = sqrt(2*mean(position.*position));
cosphi(3) = 2*mean(voltage.*position)/avgvolt(3)/avgpos(3);
sinphi(3) = sin(acos(cosphi(3)));
subplot(3,2,3)
plot(time,position);

load sin10goodb;;
[a b] = size(data);
time = (1:b)*1/freq;
a = 1485;%1;
b = 1997;%length(position);
position = (data(1,:)*256+data(2,:))*sprocket;
voltage = voltage(a:b);
position=detrend(position);
position=(position(a:b));
time = time(a:b);
avgvolt(4) = sqrt(2*mean(voltage.*voltage));
avgpos(4) = sqrt(2*mean(position.*position));
cosphi(4) = 2*mean(voltage.*position)/avgvolt(4)/avgpos(4);
sinphi(4) = sin(acos(cosphi(4)));
subplot(3,2,4)
plot(time,position);


load sin20gooda;;
[a b] = size(data);
time = (1:b)*1/freq;
a = 1650;%1;
b = 2520;%length(position);
position = (data(1,:)*256+data(2,:))*sprocket;
voltage = voltage(a:b);
position=detrend(position);
position=(position(a:b));
time = time(a:b);
avgvolt(5) = sqrt(2*mean(voltage.*voltage));
avgpos(5) = sqrt(2*mean(position.*position));
cosphi(5) = 2*mean(voltage.*position)/avgvolt(5)/avgpos(5);
sinphi(5) = sin(acos(cosphi(5)));
subplot(3,2,5)
plot(time,position);

load sin20goodb;;
[a b] = size(data);
time = (1:b)*1/freq;
a = 1660;%1;
b = 2018;%length(position);
position = (data(1,:)*256+data(2,:))*sprocket;
voltage = voltage(a:b);
position=detrend(position);
position=(position(a:b));
time = time(a:b);
avgvolt(6) = sqrt(2*mean(voltage.*voltage));
avgpos(6) = sqrt(2*mean(position.*position));
cosphi(6) = 2*mean(voltage.*position)/avgvolt(6)/avgpos(6);
sinphi(6) = sin(acos(cosphi(6)));
subplot(3,2,6)
plot(time,position);

%%%%%%%%%%%%%%
% Parameters %
%%%%%%%%%%%%%%
w = 2*pi*[1/2.048 1/2.048 1/1.024 1/1.024 1/.512 1/.512]';
Mchain = 1.008;
Msprocket = 0.0827;
Mcart = .324;%+2*Msprocket;
Me = Mcart+Mchain;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create matrices to solve the equation  %
% Ax = B                                 %
% using a least squares approximation    %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A = [vleftavg 0 1;
    vrightavg 0 -1;
    voltleft' vl' sign(vl');
    voltright' vr' sign(vr');
    avgvolt' w.*avgpos'.*sinphi' zeros(6,1);
    zeros(6,1) w.*avgpos'.*cosphi' zeros(6,1);]

B = [0 0 0 0 0 0 0 0 -Me*w'.^2.*avgpos.*cosphi Me*w'.^2.*avgpos.*sinphi]'

%%% Least Squares Solution %%%
alpha = inv(A'*A)*A'*B