====== PID - Pneumatic Inverted Pendulum ======
<!-- Replace the above line with the name of your "How To" Tutorial e.g. How to Laser cut Your Name in Wood -->

<!-- Everywhere you see <some sentence>, replace it with the answer (be concise), and remove the < and > brackets -->

**Author:** Hyunhee Lee Email: <leeh67@unlv.nevada.edu> <!-- replace with your email address -->
\\
**Date:** Last modified on 07/07/2017
\\
**Keywords:** Pneumatic, Inverted, Pendulum
\\

<!-- Add a representative photo of your tutorial below.  Make it centered in the page -->

 **Motivation and Audience**: This tutorial is for studying and analyzing about control mechanism through inverted pendulum.  Inverted pendulum mechanism is popularly used in control field.

{{ .jpg }}


\\
\\

===== Modeling of the Pneumatic inverted pendulum =====

{{ .jpg }}
\\
There are two encoders in the cart. One is the angular encoder and the other is linear encoder.
The angular encoder measures angular displacements of a swing pendulum and the linear encoder measures linear displacements of cart.
Arduino controls the output voltage for proportional vale. 


===== Equations of motion =====
Euler-Lagrange equation is used for derive equations of motion.
{{ lego_rip_v2:rip_equ1.jpg }}
\\
The kinetic energy of the system is
{{ lego_rip_v2:rip_equ2.jpg }}
\\
  * **HW.** Derive the kinetic energy.
And the potential energy of the system is
{{ lego_rip_v2:rip_equ3.jpg }}
\\
Then the Lagrangian is same as the following.
{{ lego_rip_v2:rip_equ4.jpg }}
\\
From Euler-Lagrange equation, the equations of motion are
{{ lego_rip_v2:rip_equ5.jpg }}
\\
For linear control, we need to linearize the equations about near the equilibrium point (α = 0 ).
Assume that α is a small angle. Then,
{{ lego_rip_v2:rip_equ6.jpg }}
\\
Therefore the linearized equations of motion are
{{ lego_rip_v2:rip_equ7.jpg }}
\\
  * **HW.** Linearize the above nonlinear equations by Taylor expansion.
For state-space representation, the equations are reconstructed.
{{ lego_rip_v2:rip_equ8.jpg }}
\\
It needs to transform the input torque into the motor voltage because the torque can not be used by the input in the lego system. So the following DC motor model is needed to do that. If a motor inductance and viscous damping can be neglected,
{{ .jpg }}
\\
where, R, kt, Jm, and kb are a motor resistance, a torque constant, a motor armature inertia, and a back-emf constant, respectively.
Assume that an external torque is neglected and then the motor torque is
{{ lego_rip_v2:.jpg }}
\\
Let's substitute this motor torque into the equations of motion and set states X.
{{ lego_rip_v2:.jpg }}
\\
Therefore, the state-space representation for full-state feedback control is shown
{{ lego_rip_v2:.jpg }}
\\
  * **HW.** Derive a transfer function assuming that an output, y, is a α.
===== PID control =====

To control, PID controller is used.
{{ .jpg }}
\\
where, ref, k, and u are a reference input, a feedback gain vector, and a control input, respectively.
A control law is
{{ lego_rip_v2:.jpg }}
\\
, and since we desire that the pendulum keeps upright, the ref is equal to zero.
{{ lego_rip_v2:.jpg }}
\\
where k1, k2, k3, and k4 are gains of a swing arm angle, a pendulum angle, a swing arm angular velocity and a pendulum angular velocity.\\
Now we need to determine these gains.

===== PID for simulation =====
==== Matlab Code ====
<code java>
%% PID Control for PIP
clc;
clear;
%syms s;
s = tf('s');

m = 0.064;
M = 0.322;
l = 1.22;
g = 9.8;
IP = (m*l^2)/3;
b = 0.23; %0.44

num_p = [m*l 0];
denom_p = [(M+m)*(IP+m*l^2)-m^2*l^2 b*(IP+m*l^2) -(M+m)*m*l*g -b*m*g*l];

num_c = [(IP+m*l^2) 0 -m*l*g];
denom_c = [(M+m)*(IP+m*l^2)-m^2*l^2 b*(IP+m*l^2) -(M+m)*m*l*g -b*m*g*l 0];



Gp_p = tf(num_p,denom_p);
Gp_c = tf(num_c, denom_c);
H = 1;


Kp = 30; %%18 70
Ki = 12;  %%5 2
Kd = 18;  %%4 15

Cont = (Kp + (Ki*(1/s)) + (Kd*s));

C_p1 = Gp_p/(1+Gp_p*Cont);

C_p1 = minreal(C_p1);

</code>
<code>

tn = 0:0.01:15;
syms t
impul(t) = (-0.0597.*exp((-31.9435).*t))-(0.2074.*exp((-0.7377).*t)*2.*cos(((0.1036).*t)-98.263));
yn1 = double( subs(impul, symvar(t), tn)) ;

t = 0:0.01:15;

grid on
plot(tn,yn1)
</code>
===== Arduino code for PID =====
<code java>

char controlU = 0;

bool flag_balanceControl = true;

byte fileHandle;

sub MotorPID(float refRad, float PGain, float IGain, float DGain)
{
   float error = refRad - Mrad;
   float U = PGain*error + IGain*M_errSum - DGain*d_Mrad;

   if(U > FULL_SPEED)
   {
      controlU = FULL_SPEED;
   }
   else if(U < -FULL_SPEED)
   {
      controlU = -FULL_SPEED;
   }
   else
   {
      controlU = U;
   }

   M_errSum = M_errSum + error - 1.0*(U - controlU);
   

</code>