pid_control_-_pneumatics_inverted_pendulum
Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
pid_control_-_pneumatics_inverted_pendulum [2017/07/10 16:59] – [NXC code for LEGO NXT Brick] hyunheelee | pid_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, | + | %% PID Control for PIP |
+ | clc; | ||
+ | clear; | ||
+ | %syms s; | ||
+ | s = tf(' | ||
- | mp=((2*0.36335)+(2*1.33192)+0.56)/ | + | m = 0.064; |
- | r=140/1000; %Arm Length | + | M = 0.322; |
- | L=209/1000; %Pendulum Length | + | l = 1.22; |
- | g=9.81; | + | g = 9.8; |
- | ma=(0.66556+1.33192)/ | + | IP = (m*l^2)/3; |
- | Ja=(1/3)*ma*(r^2); % Arm moment of inertia | + | b = 0.23; %0.44 |
- | R=9.05; | + | |
- | kt=0.26; | + | |
- | kb=0.26; | + | |
- | 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) |
+ | 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; | ||
- | q3 = 50; %weight on motor velocity | ||
- | q4 = 300; | ||
- | Q = diag([q1 q2 q3 q4]); | ||
- | R = 1; | + | Gp_p = tf(num_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/ | ||
+ | |||
+ | C_p1 = minreal(C_p1); | ||
- | K = -lqr(A, | ||
- | display(K); | ||
</ | </ | ||
< | < | ||
- | K = | ||
- | -7.0711 -361.6049 -7.2061 -9.1540 | + | tn = 0: |
+ | 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, | ||
</ | </ | ||
- | |||
===== 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: | ||
| | ||
- | | ||
- | } | ||
- | |||
- | sub MotorOpenLoop(float speed) | ||
- | { | ||
- | | ||
- | { | ||
- | controlU = FULL_SPEED; | ||
- | } | ||
- | else if(speed < -FULL_SPEED) | ||
- | { | ||
- | controlU = -FULL_SPEED; | ||
- | } | ||
- | else | ||
- | { | ||
- | controlU = speed; | ||
- | } | ||
- | |||
- | | ||
- | } | ||
- | |||
- | 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; | ||
- | |||
- | | ||
- | { | ||
- | 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' | ||
- | float Gm_p = -(-7.07106781265702); | ||
- | float Gp_p = -361.604865668371; | ||
- | float Gm_v = -(-7.20613568524004); | ||
- | float Gp_v = -9.15404034187486; | ||
- | |||
- | | ||
- | |||
- | | ||
- | { | ||
- | if(flag_balanceControl == true) | ||
- | { | ||
- | | ||
- | } | ||
- | Wait(MS_20); | ||
- | } | ||
- | } | ||
- | |||
- | task Display() | ||
- | { | ||
- | | ||
- | { | ||
- | TextOut(0, LCD_LINE1, FormatNum(" | ||
- | TextOut(0, LCD_LINE2, FormatNum(" | ||
- | TextOut(0, LCD_LINE3, FormatNum(" | ||
- | TextOut(0, LCD_LINE4, FormatNum(" | ||
- | TextOut(0, LCD_LINE5, FormatNum(" | ||
- | TextOut(0, LCD_LINE6, FormatNum(" | ||
- | Wait(MS_200); | ||
- | } | ||
- | } | ||
- | |||
- | task WriteData() | ||
- | { | ||
- | short bytesWritten; | ||
- | | ||
- | long cnt = 0; | ||
- | |||
- | | ||
- | |||
- | | ||
- | { | ||
- | ++cnt; | ||
- | write = NumToStr(cnt); | ||
- | write = StrCat(write, | ||
- | WriteLnString(fileHandle, | ||
- | Wait(MS_5); | ||
- | } | ||
- | } | ||
- | |||
- | task main() | ||
- | { | ||
- | | ||
- | | ||
- | | ||
- | // | ||
- | // | ||
- | // | ||
- | } | ||
</ | </ | ||
pid_control_-_pneumatics_inverted_pendulum.txt · Last modified: 2017/07/10 17:18 by hyunheelee