Differences

This shows you the differences between two versions of the page.

--- lego_rip [2017/05/19 15:27] – [LQR for the gains] sangsinpark
+++ lego_rip [2017/06/15 10:10] (current) – [NXC code for LEGO NXT Brick] sangsinpark
@@ Line 8: / Line 8: @@
 **Date:** Last modified on 05/19/2017
 \\
-**Keywords:** RIP
+**Keywords:** Rotary inverted pendulum, LEGO brick, and NXC
 \\
@@ Line 36: / Line 36: @@
 {{ lego_rip_v2:rip_equ2.jpg }}
 \\
-  * HW. Derive the kinetic energy.
+  * **HW.** Derive the kinetic energy.
 And the potential energy of the system is
 {{ lego_rip_v2:rip_equ3.jpg }}
@@ Line 53: / Line 53: @@
 {{ lego_rip_v2:rip_equ7.jpg }}
 \\
-  * HW. Linearize the above nonlinear equations by Taylor expansion.
+  * **HW.** Linearize the above nonlinear equations by Taylor expansion.
 For state-space representation, the equations are reconstructed.
 {{ lego_rip_v2:rip_equ8.jpg }}
@@ Line 70: / Line 70: @@
 {{ lego_rip_v2:rip_equ11.jpg }}
 \\
+  * **HW.** Derive a transfer function assuming that an output, y, is a α.
 ===== Full state feedback control =====
@@ Line 88: / Line 88: @@
 ===== LQR for the gains =====
 ==== Matlab Code ====
-<code>
+<code java>
 clear, clc, close all
@@ Line 101: / Line 101: @@
 kb=0.26;   %DC-motor back-emf constant
-Am=[0 0 1 0;
+A=[0 0 1 0;
 0 0 1;
 mp*r*g/Ja -kt*kb/Ja/R 0;
 -(mp*r^2 + Ja)*g/Ja/L -kt*kb*r/Ja/L/R 0];
-Bm=[0; 0; kt/Ja/R; kt*r/Ja/L/R];
+B=[0; 0; kt/Ja/R; kt*r/Ja/L/R];
 q1 = 50;  %Weight on motor position
@@ Line 117: / Line 117: @@
 R = 1;
-K = -lqr(Am,Bm,Q,R);
+K = -lqr(A,B,Q,R);
+display(K);
+</code>
+<code>
+K =
-Gm_p=K(1,1);
+   -7.0711 -361.6049   -7.2061   -9.1540
-Gp_p=K(1,2);
-Gm_v=K(1,3);
-Gp_v=K(1,4);
 </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>