(*by Nasser M. Abbasi, version December 27, 2013 removed FinishDynamic[] was causing a problem*) Manipulate[ tick; If[needToUpdateSystem, ( needToUpdateSystem = False; (* generate the stepResponse once, only when the parameters change *) plant@updateStepResponse[s, t, 0, simulationTime]; feedBack@updateStepResponse[s, t, 0, simulationTime]; stepResponsePlot = feedBack@getPlot[s, t, simulationTime, showPlantResponse]; bodePlot = feedBack@getBodePlot[]; lowerGrid = Rasterize[(*rastersize for faster rendering*) Text@Grid[{ { Grid[{{ Text@Grid[{ {Style["plant", 11], Style["closed loop", 11]}, {TraditionalForm@plant@getPolynomial[s], TraditionalForm@feedBack@getPolynomial[s]} }, Spacings -> {0, 0.3}, Frame -> All, FrameStyle -> Directive[Thickness[.001], Gray] ]}, {stepResponsePlot} }, Spacings -> {0, 0}, Alignment -> Center, Frame -> None ], Grid[{ {bodePlot[[1]]}, {bodePlot[[2]]} }, Spacings -> {0, 0} ] } }, Spacings -> {0, 0}, Frame -> None ] ]; nyquistPlot = Rasterize@ NyquistPlot[feedBack@getOpenLoopTF[s], ImageSize -> {0.42*ContentSizeW, 0.3*ContentSizeH}, AspectRatio -> 1, ImagePadding -> 0, ImageMargins -> 0, PlotLabel -> Text@Style["open loop Nyquist", 11]]; currentTime = 0 ) ]; finalPlot = Grid[{ { Grid[{ {Grid[{{Text@Style["plant", 11], makeMechanicalSystem[ plant@getStepResponse[] /. t -> currentTime]}, {Text@Style[Column[{"closed", "loop"}], 11], makeMechanicalSystem[ feedBack@getStepResponse[] /. t -> currentTime]}}], nyquistPlot } }, Frame -> All, FrameStyle -> Directive[Thickness[.005], Gray], Spacings -> {0.2, 0}] }, {lowerGrid, SpanFromLeft} }, Spacings -> {0, 0}, Alignment -> Center, Frame -> None, FrameStyle -> Directive[Thickness[.005], Gray] ]; If[doSimulation, ( If[currentTime < simulationTime, ( If[currentTime + delta >= simulationTime, ( currentTime = simulationTime; ), ( currentTime += delta ) ]; tick += $MachineEpsilon ) ] ) ]; finalPlot , (*------ start controls ------------------*) Grid[{ {Button[Text[Style["run", 12]], { currentTime = 0; doSimulation = True; tick += $MachineEpsilon }, ImageSize -> {60, 28}], Button[Text[Style["stop", 12]], { currentTime = 0; doSimulation = False; tick += $MachineEpsilon }, ImageSize -> {50, 28}], Dynamic[ Text@Style[ Grid[{{"time"}, {currentTime}}, Spacings -> {0.1, 0.1}, Alignment -> Center], 11]] } }, Spacings -> {.2, 0}, Alignment -> Center], Grid[{ {Text@Style["time", 11], Manipulator[Dynamic[simulationTime, { simulationTime = #; needToUpdateSystem = True; currentTime = 0; tick += del} &], {1, 50, 1}, ImageSize -> Tiny, ContinuousAction -> False], Text@Style[Dynamic@padIt3[simulationTime, {2, 0}], 11] }, {Text@Style["slow", 11], Manipulator[ Dynamic[delta, {delta = #; currentTime = 0; tick += del} &], {0.1, 3, 0.1}, ImageSize -> Tiny, ContinuousAction -> False], Text@Style["fast", 11] } }, FrameStyle -> Directive[Thickness[.005], Gray], Frame -> True, Spacings -> {0.2, 0}], Grid[{ {Text@Style["\[Zeta]", 11], Manipulator[ Dynamic[zeta, {zeta = #; needToUpdateSystem = True; currentTime = 0; doSimulation = False; plant@setZeta[zeta]; tick += del} &], {0.01, 2.0, 0.01}, ImageSize -> Tiny, ContinuousAction -> False], Text@Style[Dynamic@padIt2[zeta, {3, 2}], 11] }, {Text@Style["\!\(\*SubscriptBox[\(\[Omega]\), \(n\)]\)", 11], Manipulator[ Dynamic[wn, {wn = #; needToUpdateSystem = True; currentTime = 0; doSimulation = False; plant@setwn[wn]; tick += del} &], {0.1, 2., 0.1}, ImageSize -> Tiny, ContinuousAction -> False], Text@Style[Dynamic@padIt2[wn, {2, 1}], 11] }, {Text@Style["DC gain", 11], Manipulator[ Dynamic[dcGain, {dcGain = #; needToUpdateSystem = True; currentTime = 0; doSimulation = False; plant@setGain[dcGain]; tick += del} &], {0.01, 1.5, 0.01}, ImageSize -> Tiny, ContinuousAction -> False], Text@Style[Dynamic@padIt2[dcGain, {3, 2}], 11] } }, FrameStyle -> Directive[Thickness[.005], Gray], Frame -> True, Spacings -> {0.2, 0}], Grid[{ { Row[{PopupMenu[ Dynamic[controllerType, {controllerType = #; needToUpdateSystem = True; controller@setType[controllerType]; currentTime = 0; doSimulation = False; tick += del} &], { "P" -> Style["P", 11], "PI" -> Style["PI", 11], "PD" -> Style["PD", 11], "PID" -> Style["PID", 11] }, ImageSize -> All ], Row[{Style[" show plant ", 11], Checkbox[ Dynamic[showPlantResponse, {showPlantResponse = #; needToUpdateSystem = True; tick += del} &]]}] }], SpanFromLeft }, {Text@Style["P", 11], Manipulator[ Dynamic[propertional, {propertional = #; needToUpdateSystem = True; doSimulation = False; controller@setKp[propertional]; currentTime = 0; tick += del} &], {0.001, 10, 0.001}, ImageSize -> Tiny, ContinuousAction -> False ], Text@Style[Dynamic@padIt2[propertional, {5, 3}], 11] }, {Text@Style["I", 11], Manipulator[ Dynamic[integral, {integral = #; needToUpdateSystem = True; doSimulation = False; controller@setKi[integral]; currentTime = 0; tick += del} &], {0.0, 5, 0.001}, ImageSize -> Tiny, ContinuousAction -> False, Enabled -> Dynamic[controllerType == "PI" || controllerType == "PID"] ], Text@Style[Dynamic@padIt2[integral, {4, 3}], 11] }, {Text@Style["D", 11], Manipulator[ Dynamic[derivative, {derivative = #; needToUpdateSystem = True; doSimulation = False; controller@setKd[derivative]; currentTime = 0; tick += del} &], {0.0, 5, 0.001}, ImageSize -> Tiny, ContinuousAction -> False, Enabled -> Dynamic[controllerType == "PD" || controllerType == "PID"] ], Text@Style[Dynamic@padIt2[derivative, {4, 3}], 11] } }, Alignment -> Left, FrameStyle -> Directive[Thickness[.005], Gray], Frame -> True, Spacings -> {0.3, 0.2} ], Grid[{ {Text@Style["poles locations in s-plane", 11]}, {Dynamic[Show[ polesPlot[feedBack@getPoles[], Blue], If[showPlantResponse, polesPlot[plant@getPoles[s], Black], {}] , ImageMargins -> 0, ImagePadding -> {{20, 5}, {15, 0}}, ImageMargins -> 0, AspectRatio -> 1 ] ]}, {Dynamic[feedBack@formatClosedLoopPoles[]]} }, Alignment -> Center, FrameStyle -> Directive[Thickness[.002], Gray], Frame -> True, Spacings -> {0, 0} ], {{tick, 0}, None}, {{del, $MachineEpsilon}, None}, {{doSimulation, False}, None}, {{currentTime, 0}, None}, {{propertional, 10.0}, None}, {{integral, 3.730}, None}, {{derivative, 0.697}, None}, {{finalPlot, {}}, None}, {{controllerType, "PID"}, None}, {{delta, .2}, None}, {{needToUpdateSystem, True}, None}, {{showPlantResponse, True}, None}, {{simulationTime, 20}, None}, {{zeta, 0.77}, None}, {{wn, 1.0}, None}, {{dcGain, 1.0}, None}, {{lowerGrid, {}}, None}, {{bodePlot, {}}, None}, {{stepResponsePlot, {}}, None}, {{nyquistPlot, {}}, None}, {plant, None}, {controller, None}, {feedBack, None}, TrackedSymbols :> {tick}, SynchronousUpdating -> False, ContinuousAction -> False, SynchronousInitialization -> True, Alignment -> Center, ImageMargins -> 0, FrameMargins -> 0, Paneled -> True, Frame -> False, ControlPlacement -> Left, Initialization :> {ContentSizeW = 410; ContentSizeH = 470 ; (*-----------------------------------------*) padIt2[v_?(NumericQ[#] && Im[#] == 0 &), f_List] := AccountingForm[Chop[N@v] , f, NumberSigns -> {"", ""}, NumberPadding -> {"0", "0"}, SignPadding -> True]; (*-----------------------------------------*) padIt3[v_?(NumericQ[#] && Im[#] == 0 &), f_List] := AccountingForm[Chop[N@v] , f, NumberSigns -> {"", ""}, NumberPadding -> {"0", "0"}, SignPadding -> True, NumberPoint -> ""]; (*-----------------------------------------*) (*Plant class*) plantClass[$s_, $t_, $from_, $to_, $zeta_, $wn_, $gain_] := Module[{zeta = $zeta, wn = $wn, gain = $gain, stepResponse, self}, self@getStepResponse[s_, t_, from_, to_] := ( First@OutputResponse[self@getTF[s], UnitStep[t], {t, from, to}] ); self@setZeta[v_] := ( zeta = v; ); self@setwn[v_] := ( wn = v; ); self@setGain[v_] := ( gain = v; ); self@getTF[s_] := ( TransferFunctionModel[(wn^2*gain)/(s^2 + 2*zeta*wn*s + wn^2), s] ); self@getPolynomial[s_] := (wn^2*gain)/(s^2 + 2*zeta*wn*s + wn^2); self@updateStepResponse[s_, t_, from_, to_] := stepResponse = self@getStepResponse[s, t, from, to]; self@getStepResponse[] := stepResponse; self@getPoles[s_] := TransferFunctionPoles[self@getTF[s]]; stepResponse = self@getStepResponse[$s, $t, $from, $to]; self ]; (*-----------------------------------------*) (*controller class*) controllerClass[$type_, $kp_, $ki_, $kd_] := Module[{type = $type, kp = $kp, ki = $ki, kd = $kd, self}, self@setType[v_] := type = v; self@setKp[v_] := kp = v; self@setKi[v_] := ki = v; self@setKd[v_] := kd = v; self@getPolynomial[s_] := ( Which[type == "PID", kp + ki/s + kd*s, type == "PD", kp + kd*s, type == "PI", kp + ki/s, type == "P", kp ] ); self ]; (*-----------------------------------------*) (*closed loop feedback class*) feedBackClass[$s_, $t_, $from_, $to_] := Module[{getOpenLoopPolynomial, stepResponse, self}, getOpenLoopPolynomial[s_] := controller@getPolynomial[s]*plant@getPolynomial[s]; self@getStepResponse[s_, t_, from_, to_] := First@OutputResponse[ self@getTF[s], UnitStep[t], {t, from, to}]; self@getOpenLoopTF[s_] := TransferFunctionModel[getOpenLoopPolynomial[s], s]; self@getOpenLoopPhaseMargins[s_] := PhaseMargins[self@getOpenLoopTF[s]]; self@getOpenLoopGainMargins[s_] := GainMargins[self@getOpenLoopTF[s]]; self@getPolynomial[s_] := Module[{p = getOpenLoopPolynomial[s]}, N@Simplify[p/(1 + p)] ]; self@getTF[s_] := TransferFunctionModel[self@getPolynomial[s], s]; self@updateStepResponse[s_, t_, from_, to_] := stepResponse = self@getStepResponse[s, t, from, to]; self@getStepResponse[] := stepResponse; self@getPoles[] := Module[{s}, TransferFunctionPoles[self@getTF[s]] ]; self@isStable[] := Module[{poles = self@getPoles[]}, poles = Flatten[Re[poles]]; If[Length[Cases[poles, n_ /; n > 0, {}]] > 0, False, True] ]; (**-----------------------------------------**) self@getPlot[s_, t_, simulationTime_, showPlantResponse_] := Module[{plotOptions, line, plantPlot, closedLoopPlot, plantPlotStyle = {Black, Thin}, closedLoopPlotStyle = {Blue, Thin}}, plotOptions = {AxesOrigin -> {0, 0}, PlotRange -> {{0, simulationTime}, All}, ImagePadding -> {{30, 15}, {40, 20}}, Frame -> True, ImageSize -> {0.65*ContentSizeW, 0.6*ContentSizeH}, AspectRatio -> 1.1, ImageMargins -> 0, GridLines -> Automatic, GridLinesStyle -> Directive[Thickness[.001], Gray, Dashed], Axes -> False, Exclusions -> None}; line = ListPlot[{{0, 1}, {simulationTime, 1}}, PlotStyle -> Red, Joined -> True]; plantPlot = Plot[plant@getStepResponse[], {t, 0, simulationTime}, Evaluate@plotOptions, PlotStyle -> plantPlotStyle]; closedLoopPlot = Plot[self@getStepResponse[], {t, 0, simulationTime}, Evaluate@plotOptions, PlotStyle -> closedLoopPlotStyle, FrameLabel -> {{None, None}, {Text@Style[Row[{Style["t", Italic], " sec"}], 11], Text@Style["step response", 11]}}, Epilog -> { If[self@isStable[], Text[Framed[Style["stable", Black, 12], FrameMargins -> .1], Scaled[{.06, .05}], {-1, 0}], Text[Framed[Style["unstable", Red, 12], FrameMargins -> .1], Scaled[{.06, .05}], {-1, 0}] ], makePlotLegend[{"closed loop", If[showPlantResponse, "plant", ""]}, (Graphics[{Directive[#], Line[{{-4, 0}, {4, 0}}]}]) & /@ {closedLoopPlotStyle, If[showPlantResponse, plantPlotStyle, White]}, {0.56, 0.2}, 14, 12, "Arial"] } ]; Show[closedLoopPlot, If[showPlantResponse, plantPlot, {}], line, ImageMargins -> 0] ]; (**-----------------------------------------**) self@getBodePlot[] := Module[{bodePlotTitle, phasePlotTitle, pm, gm, s}, pm = Last@Sort@self@getOpenLoopPhaseMargins[s]; gm = Last@Sort@self@getOpenLoopGainMargins[s]; bodePlotTitle = Text@Style[ Grid[{ {"open loop magnitude plot", SpanFromLeft}, {"crossover (rad/sec)", "margin (db)"}, {gm[[1]], 20*Log[10, gm[[2]]]} }, Frame -> All, FrameStyle -> Directive[Thickness[.001], Gray], Spacings -> {0.2, 0.15}, Alignment -> Center], 11]; phasePlotTitle = Text@Style[ Grid[{ {"open loop phase plot", SpanFromLeft}, {"crossover (rad/sec)", "margin (deg)"}, {pm[[1]], 180/Pi pm[[2]]} }, Frame -> All, FrameStyle -> Directive[Thickness[.001], Gray], Spacings -> {0.2, 0.15}, Alignment -> Center], 11]; BodePlot[ self@getOpenLoopTF[s], StabilityMargins -> {True, True}, StabilityMarginsStyle -> {Red, Red}, GridLines -> Automatic, ImagePadding -> {{{30, 5}, {30, 5}}, {{30, 5}, {30, 5}}}, ImageMargins -> {0, 0.1}, PlotLabel -> {bodePlotTitle, phasePlotTitle}, PlotLayout -> "List" ] ]; (**-----------------------------------------**) self@formatClosedLoopPoles[] := Module[{tbl = Table["", {4}], poles = Flatten@self@getPoles[]}, tbl[[1 ;; Length[poles]]] = poles; Text@Style[Grid[{ {Text@Style["closed loop poles", 11]}, {Column[tbl]}}, Frame -> None, Spacings -> {0, 0.2}, Alignment -> Center], 11] ]; stepResponse = self@getStepResponse[$s, $t, $from, $to]; self ]; (*-----------------------------------------*) polesPlot[poles_, plotStyle_] := Module[{polesPoints, plotOptions}, plotOptions = {AxesOrigin -> {0, 0}, ImagePadding -> {{0, 0}, {0, 5}}, Frame -> True, ImageMargins -> 0, AspectRatio -> 1, GridLines -> Automatic, GridLinesStyle -> Directive[Thickness[.001], Gray, Dashed]}; polesPoints = makePolesAndZerosCoordinates[poles]; Show[ListPlot[polesPoints, AxesOrigin -> {0, 0}, PlotMarkers -> Style["\[CircleTimes]", plotStyle, 12], Evaluate@plotOptions ], PlotRange -> { { Min[-1.2, 1.2*Min[polesPoints[[All, 1]] ] ], Max[1.2, 1.2*Max[polesPoints[[All, 1]]] ]}, { Min[-1.2, 1.2*Min[polesPoints[[All, 2]] ] ], Max[1.2, 1.2*Max[polesPoints[[All, 2]]] ]} }, ImageSize -> {.35*ContentSizeW, .3*ContentSizeH}, ImageMargins -> 0, ImagePadding -> 0 ] ]; (*-----------------------------------------*) makePolesAndZerosCoordinates[points_] := Module[{xy}, xy = Flatten[Replace[ points, {Complex[x_, y_] :> {x, y}, x_?NumericQ :> {x, 0}}, {3}], 1]; Cases[xy, {_?NumericQ, _?NumericQ}, {2}] ]; (*-----------------------------------------*) makePlotLegend[names_, markers_, origin_, markerSize_, fontSize_, font_] := Module[{i, idx}, idx = Flatten[Position[names, _?(Not[# == ""] &)]]; Join @@ Table[ { Text[ Style[names[[idx[[i]]]], FontSize -> fontSize, font], Offset[{1.5*1.5*markerSize, -(i - 0.5)* Max[markerSize, fontSize]*1.25}, Scaled[origin]], {-1, 0} ], Inset[ Show[ markers[[idx[[i]]]], ImageSize -> 3*markerSize ], Offset[ {markerSize/2, -(i - 0.5)*Max[markerSize, fontSize]*1.25}, Scaled[origin] ], {0, 0}, Background -> Directive[Opacity[0], White] ] } , {i, 1, Length[idx]} ] ]; (*-----------------------------------------*) (*Thanks to Árpád Kósa for this function below which draws the \ spring. From a demo found at WRI site*) rugo[xkezd_, ykezd_, xveg_, yveg_] := Module[{step = 12, szel = 0.25 (*spring width*), hx, hy, veghossz = 0.3, hossz, dh, i}, { hx = xveg - xkezd; hy = yveg - ykezd; hossz = Sqrt[hx^2 + hy^2]; dh = (hossz - 2*veghossz)/step; {xkezd, ykezd}}~ Join~{{xkezd + hx*(dh + veghossz)/hossz, ykezd + hy*(dh + veghossz)/hossz}}~Join~ Table[ If[OddQ[i], {xkezd + hx*(i*dh + veghossz)/hossz + hy*szel/hossz, ykezd + hy*(i*dh + veghossz)/hossz - hx*szel/hossz}, {xkezd + hx*(i*dh + veghossz)/hossz - hy*szel/hossz, ykezd + hy*(i*dh + veghossz)/hossz + hx*szel/hossz}], {i, 2, (step - 2)}]~ Join~{{xkezd + hx*((step - 1)*dh + veghossz)/hossz, ykezd + hy*((step - 1)*dh + veghossz)/hossz}}~ Join~{{xveg, yveg}} ]; (*-----------------------------------------*) makeMechanicalSystem[y$_] := Module[{a1 = 1, a2, a3 = .7, a4, a6 = .3, annot, a0 = .7, y = y$}, a2 = .15*a0; a4 = .3*a0; (*add virtual stop guards for unstable system*) If[y > 1.5, y = 1.5]; If[y < -0.2, y = -0.2]; annot = Row[{PaddedForm[(Chop[N@y, 10^-5]), {6, 6}, NumberSigns -> {"", ""}, NumberPadding -> {"0", "0"}]}]; Graphics[{ {EdgeForm[Directive[Thick, Black]], GrayLevel[0.9], Rectangle[{0, 0}, {1.5 + a0, a1}]}, {GrayLevel[0.5], Opacity[.5], Rectangle[{y + a0, 0}, {y + a0 + 0.05, a1}]}, {Black, Line[{{a4, a3}, {y + a0, a3}}] },(*damper to mass line*) {Black, Line[{{0, a3}, {a2, a3}}] },(*bottom of damper to left wall*) {Black, Line[{{a2, .9 a3}, {a2, 1.1 a3}}] },(*bottom of damper*) {Black, Line[{{a2, .9 a3}, {a4, .9 a3}}] },(* bottom edge of damper dash*) {Black, Line[{{a2, 1.1 a3}, {a4, 1.1 a3}}] },(* top edge of damper dash*) {Black, Line[{{0, a6}, {a2, a6}}] },(*line from left wall to start of spring part*) {Blue, Thin, Dotted, Line[{ {a0 + 1, 0}, {a0 + 1, a1}}] },(*step response reference*) {Blue, Thin, Dotted, Line[{{a0, 0}, {a0, a1}}]},(*0 initial position line*) {Text[Style[annot, 11], {y + a0, 1.1*a1}, {0, 0}] }, {Text[ Row[{Style["x", Italic], Style[" = 0", 11]}], {a0, -0.1}, {0, 0}] }, {Text[ Row[{Style["x", Italic], Style[" = 1", 11]}], {a0 + 1, -0.1}, {0, 0}] }, {Red, Line[rugo[0, a6, y + a0, a6]]} }, PlotRange -> {{-0.01, 2.3}, {-.21, 1.2 a1}}, Axes -> False, AxesOrigin -> {0, 0}, ImageMargins -> 0, ImagePadding -> 0, ImageSize -> {0.58*ContentSizeW, 0.15*ContentSizeH}, AspectRatio -> .3] ]; (*make plant, controller and feedback objects*) plant = plantClass[s, t, 0, 20, .77, 1, 1]; controller = controllerClass["PID", 10.0, 3.730, 0.697]; feedBack = feedBackClass[s, t, 0, 20]; } ]