%% ECE 179D Segway Lab Template File
%This is a script to design a controller for the segway robot
%The first section computes the non-linear equations of motion
%The second section (with a little manual algebra first) computes a state
%space model and then designs an LQR controller

%IMPORTANT: Turn in your completed version of this M-file as well as output
%showing the following things:

% 1- A pretty printed simplified version of EQ1 and EQ2. pretty(simple(EQ1))
% 2- These matrices related to state space: M, Am, Bm, A, B
% 3- These matrices and values related to LQR: Q,R,K
% 4- The open-loop and closed loop pole locations (4 each)

%ALSO IMPORTANT: Your lab report will also need data showing a perturbation
%response on the robot. See lab handout. Do not forget this.

%ADDITIONALLY IMPORTANT: Remember to SAVE YOUR DATA and m files and
%simulink block diagrams. Do not leave them on the lab computer. Check that
%the data exists before leaving.

clear all;
close all;
make_filters;


%% Symbolic
% define symbols, for symbolic differentiation:
syms g                          % gravity
syms Rw L                       % wheel radius, length from hub to Mb
syms mb Jb mw Jw                % mass and inertia for body and wheel
syms tau                        % control input (torque)

% define symbols for the angular degrees of freedom and their derivatives:
syms theta dtheta d2theta phi dphi d2phi     % both angles are ABSOLUTE

% define (x,y) coordinates of each mass:
xw = Rw*phi
xb = xw + L*sin(theta)
yb = L*cos(theta)

% define Generalized Coordinates:
GC = {phi, theta};

% define positional derivatives wrt time:
dxw = fulldiff(xw,GC)
dxb = fulldiff(xb,GC)
dyb = fulldiff(yb,GC)

% Write an expression for the KINETIC CO-ENERGY:
Tstar = .5*mb*(dxb^2 + dyb^2) + ...
    .5*mw*(dxw^2) + ...
    .5*Jb*(dtheta^2) + ...
    .5*Jw*(dphi^2);
% ...and for the POTENTIAL ENERGY:
V = mb*g*L*cos(theta);

% Below is the Lagrangian
Lag =                                                                       %<-- Fill in here!

% For each DOF, solve for the EOM using fulldiff and diff:
eq1 =                                                                       %<-- Fill in here!
eq2 =                                                                       %<-- Fill in here!

EQ1 = simplify(eq1);
EQ2 = simplify(eq2);

pretty(EQ1)
pretty(EQ2)


%% Numeric Section

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       DEFINE PHYSICAL PARAMETERS                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Related to Motor:
Kt = .385;      % was different than Kb in Legway code? this is square root Kb*Kt
nb = 1;         % scale factor for output effort (leave as 1)
f_extra = 0.04; % to account for extra friction
Klego = 0.23;   % rad/sec ss velocity, for power level increase of 1.0
R = 6.7;        % resistance of motor, in Ohms

%Related to Robot Geometry:
Lboxedge =   %approximate body height [m]                                   %<-- Fill in here!
L =          %height from wheel axis to body COM [m]                        %<-- Fill in here!
Rw =         %radius of wheel [m]                                           %<-- Fill in here!

%Related to Mass (and gravity)
mb =        	%mass of body [kg]                                          %<-- Fill in here!
mw =            %.062; %mass of both wheels combined [kg]                   %<-- Fill in here!
g = 9.81;       %gravity [m/s^2]                                            

%Compute the inertias and a damping term for motor + extra misc. damping
Jb = (1/12)*mb*Lboxedge^2;  %inertia of body 
Jw = .5*mw*Rw^2;            %inertia of wheel
Bloss = Kt*Kt/R + f_extra;  %lump all the damping together


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       SOLVE FOR STATE SPACE MODEL                     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%The general idea is to rewrite EQ1 and EQ2 in a way that is easy to solve
%for the accelerations.  
%Step (1): Rearrange and factor equations into this form
%Step (1a): Linearize using small angle approximation (sin(t)=>t cos(t)=>1)
%Step (2): Multiple by M^-1 to isolate Xdot
%Step (3): Read off the A and B matrix to get the state space version

%  (1)      M * Xdot = Am*X + Bm*u
%  (2)      Xdot = M^-1 * Am * X + M^-1*Bm*u
%                  |-------|       |-----|
%  (3)      Xdot =     A * X    +     B * u          <--STATE SPACE!


%Define the M Matrix
M =                                                                         %<-- Fill in here! (HINT: 4X4 MATRIX, SEE NOTES)

% Am appears on opposite side of equation than M
Am =                                                                        %<-- Fill in here! (HINT: also 4X4)
Bm = 2*   %The factor of 2 is because there are 2 motors                    %<-- Fill in here! (HINT: 4X1) 

%Solve for the A and B matrices. Write down C and D.
A =                                                                         %<-- Fill in here!
B =                                                                         %<-- Fill in here!
C =                                                                         %<-- Fill in here!
D =                                                                         %<-- Fill in here!
sys_segway = ss(A,B,C,D); %Define a state space model (similar idea to tf(...)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                          DESIGN LQR CONTROLLER                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Q and R tell the LQR algorithm how much you care about errors in each
%state and how much control effort you are willing to apply
Q = diag([1 1 1 1]);                                                        %<-- Modify these until it works
R = 1;                                                                      %<-- Fill in here!

%Compute the state feedback vector K
K_LQR = lqr(sys_segway.a,sys_segway.b,Q,R)

%Check the open-loop pole locations
ol_poles =                                                                  %<-- Solve for the open loop poles

%Check the closed-loop pole locations
cl_poles =                                                                  %<-- Solve for the closed loop poles






%    _________________________________
%   |:::::::::::::;;::::::::::::::::::|
%   |:::::::::::'~||~~~``:::::::::::::|
%   |::::::::'   .':     o`:::::::::::|
%   |:::::::' oo | |o  o    ::::::::::|
%   |::::::: 8  .'.'    8 o  :::::::::|
%   |::::::: 8  | |     8    :::::::::|
%   |::::::: _._| |_,...8    :::::::::|
%   |::::::'~--.   .--. `.   `::::::::|
%   |:::::'     =8     ~  \ o ::::::::|
%   |::::'       8._ 88.   \ o::::::::|
%   |:::'   __. ,.ooo~~.    \ o`::::::|
%   |:::   . -. 88`78o/:     \  `:::::|
%   |::'     /. o o \ ::      \88`::::|   "He will save his data or die."
%   |:;     o|| 8 8 |d.        `8 `:::|
%   |:.       - ^ ^ -'           `-`::|
%   |::.                          .:::|
%   |:::::.....           ::'     ``::|
%   |::::::::-'`-        88          `|
%   |:::::-'.          -       ::     |
%   |:-~. . .                   :     |
%   | .. .   ..:   o:8      88o       |
%   |. .     :::   8:P     d888. . .  |
%   |.   .   :88   88      888'  . .  |
%   |   o8  d88P . 88   ' d88P   ..   |
%   |  88P  888   d8P   ' 888         |
%   |   8  d88P.'d:8  .- dP~ o8       |   
%   |      888   888    d~ o888    LS |
%   |_________________________________|
