2. Introduction: Extending Mathematica to Solve Control Problems

eq1=Fx==m xc''[t]

eq2=Fy - m g==m yc''[t]

eq3=(1/2) Fy L Sin[θ[t]] - 
          (1/2) Fx L Cos[θ[t]]==J θ''[t]

J = ((m (L^2))/12)

eq4=fx - Fx==M X''[t]

xc[t_] = X[t] + (1/2) L Sin[θ[t]]

yc[t_] = (1/2) L Cos[θ[t]]

Eliminate[{eq1, eq2, eq3, eq4},{Fx,Fy,X''[t]}]

Solve[%,θ''[t]]

sln = %[[1, 1]]

x={θ[t],θ'[t]};

u = {fx}; y = {θ[t]};

D[x, t]

f = % /. sln

h = y

<< ControlSystems`

ss=Linearize[f,h,{{θ[t], 0},{θ'[t],0}},{{fx, 0}}]

Factor /@ %

TraditionalForm[%]

k = StateFeedbackGains[ss, {p1, p2}]

a = ss[[1]]; 
    b = ss[[2]];

Eigenvalues[a - b . k]

numericValues = {m -> 2. , M -> 8. , L -> 1. , 
        g -> 9.8, p1 -> -4. , p2 -> -5. }

nss = ss /. numericValues

Q = IdentityMatrix[2]

R = {{1}}

LQRegulatorGains[nss, Q, R]

Eigenvalues[a - b . % /. numericValues]

(Subscript[θ, 0]) = { .5, 1. , 1.2}

StateFeedbackConnect[ss, k] // Simplify

OutputResponse[%, 0, t, 
      InitialConditions -> {angle, 0}]

plot = 
        Plot[Evaluate[% /. numericValues /. angle -> (Subscript[θ, 0])], {t, 0, 4}, 
          PlotStyle -> RGBColor[1, 0, 0]];

feedbackRules = 
      Thread[u -> -k . 
                x] /. numericValues

sln

de = Equal @@ 
            sln /. numericValues /. feedbackRules

(Subscript[Θ, 0])=NDSolve[{de,θ[0] == #,θ'[0]==0},{θ},{t, 0, 4}]&/@(Subscript[θ, 0])

SetOptions[Plot, 
        PlotStyle -> {Thickness[ .001], 
            Dashing[{ .025,  .0075,  .0075,  .0075}], Dashing[{ .01,  .01}]}, 
        Frame -> {Automatic, Automatic, None, None}, 
        FrameLabel -> {"Time (s)", None}];

plot1 = 
        Plot[Evaluate[θ[
                t] /. (Subscript[Θ, 0])], {t, 0, 4}, 
          PlotLabel -> "θ (radian)"];

Plot[Evaluate[θ'[t]/.(Subscript[Θ, 0])],{t, 0, 4},PlotLabel -> "θ' (radian/s)"];

\(\(Plot[
        Evaluate[\(fx /. feedbackRules\) /. Θ\_0], {t, 0, 4}, 
        PlotLabel -> "Input Force (Newton)", 
        PlotRange -> All];\)\)

Show[plot1, plot];

(Subscript[θ, Big])=NDSolve[{de,θ[0] == 1.25,θ'[0]==0},{θ},{t, 0, 4},MaxSteps -> 1000]

SetOptions[Plot, 
        PlotStyle -> {Thickness[ .001], 
            Dashing[{ .01,  .01}]}];

\(\(Plot[
        Evaluate[θ[
              t] /. {θ\_Big, Θ\_0[[3]]}], {t, 0, 4}, 
        PlotLabel -> "θ (radian)"];\)\)

Plot[Evaluate[θ'[t]/.{(Subscript[θ, Big]), (Subscript[Θ, 0])[[3]]}],{t, 0, 4},PlotLabel -> "θ' (radian/s)"];

\(\(Plot[
        Evaluate[\(fx /. feedbackRules\) /. {θ\_Big, Θ\_0[[3]]}], {t, 0, 4}, 
        PlotLabel -> "Input Force (Newton)", 
        PlotRange -> All];\)\)

clip = If[Abs[#] ≤ 1000. , #, Sign[#] 1000. ] &

clip /@ {-1001, -999, 999, 1001}

feedbackLtd = MapAt[clip, feedbackRules, {1, 2}]

de1 = Equal @@ 
            sln /. numericValues /. feedbackLtd

(Subscript[θ, Big1])=NDSolve[{de1,θ[0] == 1.25,θ'[0]==0},{θ},{t, 0, 4},MaxSteps -> 1000]

Plot[Evaluate[{θ[t],θ'[t]}/.(Subscript[θ, Big1])],{t, 0, 4},PlotStyle -> {Thickness[ .001], {Dashing[{ .05,  .01}], 
                Thickness[ .001]}},PlotRange -> All,PlotLabel -> "State Response"];