%% MQP Experiment, Case 2 (MQP_Experiment_Case_2.m) %%
% This program models the dynamics of a linearly-controlled, 3 craft 
% system. The system is composed of two immobile spheres on either end of a
% levitating track suspending a third, mobile sphere. The mobile sphere has 
% a constant voltage applied to it, while the other two spheres have a 
% voltage on them that is controlled by the designed controller. The 
% distance x is measured from the first sphere to the middle one, the first
% sphere is at x=0, and the two end spheres are placed a distance L away 
% from each other.

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

global Q L V1o V2o V3o K1 K2 K3

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]
xd = 1.0; % desired distance of mobile sphere (0.3 < xd < 2.7) [m]
L = 3; % distance between stationary spheres [m]
Vo = 3000; % Operating Voltage [V]
V1o = (xd^2) * Vo; % Operating Voltage on Sphere 1
V2o = Vo; % Operating Voltage on Sphere 2
V3o = ((L - xd)^2) * Vo; % Operating Voltage on Sphere 3
Q = (rho^2)/(Kc*m); % Simplifying Coefficent of Constants

%% Constant Coefficents %%

K1 = -1000000; % Velocity Error Scaling Coeffcient on Sphere 1
K2 = (30000 - Vo)*10; % Velocity Error Scaling Coeffcient on Sphere 2
K3 = 1000000; % Velocity Error Scaling Coeffcient on Sphere 3

%% Time Span %%

experimentTime = 15; % Experiement Run-Time [min]
tf = experimentTime*60; % Experiment Run-Time in seconds [s]
dt = tf/10000; % Scaled Time Step
t = (0:dt:tf); % time vector [s]

%% Initial Conditions %%

x1i(1) = 0.5; % Initial Position (Between 0.3 m and 2.7 m) [m]
x1i(2) = 0; % Initial Velocity (Cannot Excede +/- 0.1 m/s) [m/s]

x2i(1) = 2.5; % Initial Position (Between 0.3 m and 2.7 m) [m]
x2i(2) = 0; % Initial Velocity (Cannot Excede +/- 0.1 m/s) [m/s]

%% Differential Equation Solver %%

[t,x1] = ode45('xsystem_case2', t, x1i);
[t,x2] = ode45('xsystem_case2', t, x2i);

%% Plot Solution %%

figure; 

subplot(2,1,1);
plot(t, x1(:,1), 'r'); 
hold on;
plot(t,xd,'k');
title('System for xi < xd');
xlabel('Time [s]');
ylabel('Distance from First Sphere [m]');

subplot(2,1,2);
plot(t, x2(:,1), 'b'); 
hold on;
plot(t,xd,'k');
title('System for xi > xd');
xlabel('Time [s]');
ylabel('Distance from First Sphere [m]');