DASL Wiki

User Tools

Site Tools

Trace:
lego_rip

Differences

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

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
lego_rip [2017/05/19 15:50] – [Matlab Code] sangsinparklego_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 **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 }} {{ lego_rip_v2:rip_equ2.jpg }}
 \\ \\
-  * HW. Derive the kinetic energy.+  * **HW.** Derive the kinetic energy.
 And the potential energy of the system is And the potential energy of the system is
 {{ lego_rip_v2:rip_equ3.jpg }} {{ lego_rip_v2:rip_equ3.jpg }}
Line 53: Line 53:
 {{ lego_rip_v2:rip_equ7.jpg }} {{ 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. For state-space representation, the equations are reconstructed.
 {{ lego_rip_v2:rip_equ8.jpg }} {{ lego_rip_v2:rip_equ8.jpg }}
Line 70: Line 70:
 {{ lego_rip_v2:rip_equ11.jpg }} {{ lego_rip_v2:rip_equ11.jpg }}
 \\ \\
 +  * **HW.** Derive a transfer function assuming that an output, y, is a α.
 ===== Full state feedback control ===== ===== Full state feedback control =====
  
Line 88: Line 88:
 ===== LQR for the gains ===== ===== LQR for the gains =====
 ==== Matlab Code ==== ==== Matlab Code ====
-<code>+<code java>
 clear, clc, close all clear, clc, close all
  
Line 125: Line 125:
    -7.0711 -361.6049   -7.2061   -9.1540    -7.0711 -361.6049   -7.2061   -9.1540
 </code> </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>
 +
lego_rip.1495234240.txt.gz · Last modified: 2017/05/19 15:50 by sangsinpark