%% MQP Experiment, Case 1 (MQP_Experiment_Case_1.m) %%

clear all; close all; clc;
%% Parameters %%

global P xd K1 K2 g Vs Vco theta

m = 0.5; % mass of cart [kg]
Kc = 8.987551787 * 10^9; % coulomb constant [N*m^2/C^2]
rho = 0.1016; % radius of spheres [m]
P = (rho^2)/(m*Kc); % Simplifying Parameter [N*m^4/kg*C^2]
xd = 0.5; % desired distance [m]
Vo = 10000; % Operating Voltage [V]
Vs = -2*Vo; % voltage on stationary sphere [V]
g = 9.81; % Gravity [m/s^2]
tiltAngle = .01; % Track Tilt Angle [degrees]
theta = tiltAngle*(pi/180); % Track Tilt Angle [radians]
Vco = (g*(xd^2)*sin(theta))/(P*Vs); % Correction Factor for tilt [V]

%% Constant Coefficents %%
Q= 1;
R = 5;
K1 = Q*(((xd^2)/(P*Vo))+(Vo/xd));
K2 = R*((xd^2)/(P*Vo));

%% Time Span %%

t = (0:0.05:300); % time vector [s]

%% Initial Conditions %%

xi1(1) = .25; % Initial Position [m]
xi1(2) = 2; % Initial Velocity [m/s]
xi2(1) = 3; % Initial Position [m]
xi2(2) = 2; % Initial Velocity [m/s]

%% Differential Equation Solver %%

[t,x1] = ode45('xsystem', t, xi1);
[t,x2] = ode45('xsystem_tilt', t, xi1);
[t,x3] = ode45('xsystem', t, xi2);
[t,x4] = ode45('xsystem_tilt', t, xi2);

%% Plot Solution %%

figure; 

subplot(2,1,1);
plot(t, x1(:,1), 'r'); 
hold on;
plot(t,xd,'k');
title('Ideal System [Both Graphs For Case xi < xd]');
xlabel('Time [s]');
ylabel('Distance from Stationary Sphere [m]');

subplot(2,1,2);
plot(t, x2(:,1), 'r'); 
hold on;
plot(t,xd,'k');
title('Real System with 1/100 deg Tilt');
xlabel('Time [s]');
ylabel('Distance from Stationary Sphere [m]');

figure; 

subplot(2,1,1);
plot(t, x3(:,1), 'r'); 
hold on;
plot(t,xd,'k');
title('Ideal System [Both Graphs For Case xi > xd]');
xlabel('Time [s]');
ylabel('Distance from Stationary Sphere [m]');

subplot(2,1,2);
plot(t, x4(:,1), 'r'); 
hold on;
plot(t,xd,'k');
title('Real System with 1/100 deg Tilt');
xlabel('Time [s]');
ylabel('Distance from Stationary Sphere [m]');