====== LEGO Rotary Inverted Pendulum Ver. 2.0 ======
<!-- 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:** Sangsin Park Email: <sangsin.park@unlv.edu> <!-- replace with your email address -->
\\
**Date:** Last modified on 05/19/2017
\\
**Keywords:** Rotary inverted pendulum, LEGO brick, and NXC
\\

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

{{ lego_rip_v2:lego_rip_ver2.jpg }}
\\

{{youtube>tytBGp4TsPQ?medium}}
\\
\\

===== Modeling of the rotary inverted pendulum =====

{{ lego_rip_v2:rip_model.jpg }}
\\
The rotary inverted pendulum has one motor on the base joint and two encoders on each joint.
The encoders measure angular displacements of a swing arm and a pendulum.
I assume that the joint friction and the viscous damping of the motor are neglected.
At a swing arm, r, Ja, θ, and τ are a link length, a moment of inertia of the swing arm, an angular displacement, and a joint torque, respectively. At a pendulum, l, mp, α, and g are a link length, a pendulum mass, an angular displacement, and gravity acceleration, respectively.

===== 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,
{{ lego_rip_v2:rip_motor.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:rip_equ9.jpg }}
\\
Let's substitute this motor torque into the equations of motion and set states X.
{{ lego_rip_v2:rip_equ10.jpg }}
\\
Therefore, the state-space representation for full-state feedback control is shown
{{ lego_rip_v2:rip_equ11.jpg }}
\\
  * **HW.** Derive a transfer function assuming that an output, y, is a α.
===== Full state feedback control =====

To control the LEGO RIP, a full state feedback controller is used.
{{ lego_rip_v2:rip_controller.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:rip_equ12.jpg }}
\\
, and since we desire that the pendulum keeps upright, the ref is equal to zero.
{{ lego_rip_v2:rip_equ13.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.

===== LQR for the gains =====
==== Matlab Code ====
<code java>
clear, clc, close all

mp=((2*0.36335)+(2*1.33192)+0.56)/1000;  %Pendulum Mass
r=140/1000; %Arm Length
L=209/1000; %Pendulum Length
g=9.81;    %Gravity
ma=(0.66556+1.33192)/1000;  % Arm mass
Ja=(1/3)*ma*(r^2);  % Arm moment of inertia
R=9.05;   %DC-motor armature resistance
kt=0.26;   %DC-motor torque constant
kb=0.26;   %DC-motor back-emf constant

A=[0 0 1 0;
   0 0 0 1;
   0 mp*r*g/Ja -kt*kb/Ja/R 0;
   0 -(mp*r^2 + Ja)*g/Ja/L -kt*kb*r/Ja/L/R 0];

B=[0; 0; kt/Ja/R; kt*r/Ja/L/R];

q1 = 50;  %Weight on motor position
q2 = 50;   %Weight on pendulum position
q3 = 50;  %weight on motor velocity
q4 = 300;   %weight on pendulum velocity

Q = diag([q1 q2 q3 q4]);

R = 1;

K = -lqr(A,B,Q,R);
display(K);
</code>
<code>
K =

   -7.0711 -361.6049   -7.2061   -9.1540
</code>

===== NXC code for LEGO NXT Brick =====
<code java>
#define MOTOR OUT_A
#define PNDLM OUT_C
#define FULL_SPEED 100
#define D2R 0.0175 // converts degree to radian
#define R2D 57.2958 // converts radian to degree

long prev_tick = 0;
long prev_Mdeg = 0;
long prev_Pdeg = 0;
long Mdeg = 0;
long Pdeg = 0;
long M_diff = 0;
long P_diff = 0;

float dt = 0.0;
float GTime = 0.0;
float Mrad = 0.0;
float Prad = 0.0;
float d_Mrad = 0.0;
float d_Prad = 0.0;
float d_Mdeg = 0.0;
float d_Pdeg = 0.0;
float M_errSum = 0.0;

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);
   
   OnRev(MOTOR, controlU);
}

sub MotorOpenLoop(float speed)
{
   if(speed > FULL_SPEED)
   {
      controlU = FULL_SPEED;
   }
   else if(speed < -FULL_SPEED)
   {
      controlU = -FULL_SPEED;
   }
   else
   {
      controlU = speed;
   }

   OnRev(MOTOR, controlU);
}

task ReadEncoder()
{
   const int w = 4;
   float M_mov[w] = {0.0,};
   float P_mov[w] = {0.0,};
   float sum_Mmov = 0.0;
   float sum_Pmov = 0.0;
   int cnt = 0;

   while(true)
   {
      dt = (CurrentTick() - prev_tick)*0.001;
      Mdeg = MotorRotationCount(MOTOR);
      Pdeg = MotorRotationCount(PNDLM);
      M_diff = Mdeg - prev_Mdeg;
      P_diff = Pdeg - prev_Pdeg;
      prev_Mdeg = Mdeg;
      prev_Pdeg = Pdeg;

      d_Mdeg = M_diff/dt;
      d_Pdeg = P_diff/dt;

      Mrad = Mdeg*D2R;
      Prad = Pdeg*D2R;

      d_Mrad = d_Mdeg*D2R;
      d_Prad = d_Pdeg*D2R;

      prev_tick = CurrentTick();
      GTime += dt;

      Wait(MS_20);
   }
}

task BalanceControl()
{
   // A physical motor rotation direction is reversed as a model's rotation direction.
   float Gm_p = -(-7.07106781265702);     //Gain on Motor Position
   float Gp_p = -361.604865668371;        //Gain on Pendulum Position
   float Gm_v = -(-7.20613568524004);     //Gain on Motor Velocity
   float Gp_v = -9.15404034187486;       //Gain on Pendulum velocity

   Wait(MS_10);
   
   while(true)
   {
      if(flag_balanceControl == true)
      {
         MotorOpenLoop(Gp_p*Prad + Gp_v*d_Prad + Gm_p*Mrad + Gm_v*d_Mrad);
      }
      Wait(MS_20);
   }
}

task Display()
{
   while(true)
   {
      TextOut(0, LCD_LINE1, FormatNum("Frq: %.1f Hz", 1/dt), DRAW_OPT_CLEAR_LINE);
      TextOut(0, LCD_LINE2, FormatNum("P deg: %d", Pdeg), DRAW_OPT_CLEAR_LINE);
      TextOut(0, LCD_LINE3, FormatNum("M deg: %d", Mdeg), DRAW_OPT_CLEAR_LINE);
      TextOut(0, LCD_LINE4, FormatNum("Time: %.3f", GTime), DRAW_OPT_CLEAR_LINE);
      TextOut(0, LCD_LINE5, FormatNum("Flag_B: %d", flag_balanceControl), DRAW_OPT_CLEAR_LINE);
      TextOut(0, LCD_LINE6, FormatNum("Ctrl U: %d", controlU), DRAW_OPT_CLEAR_LINE);
      Wait(MS_200);
   }
}

task WriteData()
{
   short bytesWritten;
   string write;
   long cnt = 0;
   
   CreateFile("data_sp.txt", 51200, fileHandle);
   
   while(true)
   {
      ++cnt;
      write = NumToStr(cnt);
      write = StrCat(write, " ", NumToStr(GTime), " ", NumToStr(Mrad), " ", NumToStr(d_Mrad), " ", NumToStr(Prad), " ", NumToStr(d_Prad));
      WriteLnString(fileHandle, write, bytesWritten);
      Wait(MS_5);
   }
}

task main()
{
   DeleteFile("data_sp.txt");

   prev_tick = CurrentTick();
   Precedes(ReadEncoder, BalanceControl, Display);
   //Precedes(ReadEncoder, Display);
   //Precedes(ReadEncoder, WriteData, Display);
   //Precedes(ReadEncoder, BalanceControl, WriteData, Display);
}
</code>