User Tools

Site Tools


pid_control_-_pneumatics_inverted_pendulum

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
pid_control_-_pneumatics_inverted_pendulum [2017/07/10 16:59] – [NXC code for LEGO NXT Brick] hyunheeleepid_control_-_pneumatics_inverted_pendulum [2017/07/10 17:18] (current) – [Arduino code for PID] hyunheelee
Line 90: Line 90:
 ==== Matlab Code ==== ==== Matlab Code ====
 <code java> <code java>
-clear, clc, close all+%% PID Control for PIP 
 +clc
 +clear; 
 +%syms s; 
 +s = tf('s');
  
-mp=((2*0.36335)+(2*1.33192)+0.56)/1000 %Pendulum Mass += 0.064
-r=140/1000%Arm Length +0.322
-L=209/1000%Pendulum Length +1.22
-g=9.81   %Gravity +g = 9.8
-ma=(0.66556+1.33192)/1000;  % Arm mass +IP = (m*l^2)/3
-Ja=(1/3)*ma*(r^2);  % Arm moment of inertia += 0.23; %0.44
-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+num_p = [m*l 0]
-   0 0 0 1; +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];
-   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];+num_c = [(IP+m*l^2) -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];
  
-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]); 
  
-= 1;+Gp_p = tf(num_p,denom_p); 
 +Gp_c = tf(num_c, denom_c); 
 += 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);
  
-K = -lqr(A,B,Q,R); 
-display(K); 
 </code> </code>
 <code> <code>
-K = 
  
-   -7.0711 -361.6049   -7.2061   -9.1540+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> </code>
- 
 ===== Arduino code for PID ===== ===== Arduino code for PID =====
 <code java> <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; char controlU = 0;
Line 179: Line 168:
    M_errSum = M_errSum + error - 1.0*(U - controlU);    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> </code>
  
            
pid_control_-_pneumatics_inverted_pendulum.txt · Last modified: 2017/07/10 17:18 by hyunheelee