ℒ=((1/(2 ))m ((r'[t])^2)+(1/2)J ((θ'[t])^2))-(-m g r [t] Sin[θ[t]])

asumps={J->(2/3)m (R^2),θ'[t]->((r'[t])/R) };

linF=θ[t]->ArcSin[u[t]];

ℒ=ℒ/.asumps/.linF

eq=D[D[ℒ,r'[t]],t]-D[ℒ,r[t]]==0

StateSpaceModel[eq,r[t],u[t],r[t],t]
ballAndBeam=% /. g -> 9.8

port="/dev/tty.usbmodem141411";

ℳ=MicrocontrollerEmbedCode[(StateSpaceModelFull[1, 0.5, , ]),<|"Target"->"ArduinoUno","Inputs"->"A0"->"Analog","Outputs"->"Serial"|>,port]

dv=DeviceOpen["Serial",port]

parseData[{ℳ["Serial"]["StartByte"],val__}]:=ToExpression@FromCharacterCode@#&/@{{val}}
parseData[___]:={}

y={};
task=SessionSubmit@ScheduledTask[

If[DeviceExecute[dv,"SerialReadyQ"],
y=Join[y,parseData@DeviceReadBuffer[dv,"ReadTerminator"->ℳ["Serial"]["EndByte"]]];
DeviceExecute[dv,"ReadFlush"];
],

0.5
];

sensorData=;

sensorDataPlot=ListPlot[sensorData,PlotTheme->"Detailed",PlotStyle->PointSize[Medium],FrameLabel->{"voltage (volts)", "r (m)"},ImageSize->Medium]

TaskRemove[task];
DeviceClose[dv];

sensorFit=NonlinearModelFit[sensorData,(Subscript[c, 0])+(Subscript[c, 1])v+(Subscript[c, 2])(v^2)+(Subscript[c, 3])(v^3)+(Subscript[c, 4])(v^4),{(Subscript[c, 0]),(Subscript[c, 1]),(Subscript[c, 2]),(Subscript[c, 3]),(Subscript[c, 4])},v]

Show[sensorDataPlot,Plot[sensorFit[v],{v,0,3}]]

sensor=NonlinearStateSpaceModel[ {{},sensorFit[v]},{},v,SamplingPeriod->T]

servoLib=FileNameJoin[{$RootDirectory,"Applications","Arduino.app","Contents","Java","libraries","Servo","src"}]

servoOut= <|"Type"->"ExternalLibrary","IncludeFiles"-> "<Servo.h>","Utilities"->"Servo myservo","Initializations"->"myservo.attach(2)","Loop"-> {"double  angle","myservo.write(angle)"}|>;

sys=With[{angle = 81},NonlinearStateSpaceModel[{{},angle},{},u,SamplingPeriod->1]]

MicrocontrollerEmbedCode[sys,<|"Target"->"ArduinoUno", "Outputs"->servoOut,"Timer"->0|>,port,<|"Libraries"->servoLib|>]

servoData=;

servoDataPlot=ListPlot[servoData,PlotTheme->"Detailed",PlotStyle->PointSize[Medium],FrameLabel->{"θ (radians)", "angle (degrees)"},ImageSize->Medium,PlotRange->{All,{40,130}}]

servoFit=LinearModelFit[servoData,θ,θ]

servoPlot=Show[servoDataPlot,Plot[servoFit[θ],{θ,-40 °,25 °}]]

servo=NonlinearStateSpaceModel[ {{},servoFit[θ]},{},θ,SamplingPeriod->T]

pts={-0.4, 0.4};
{lLim,uLim}=servoFit/@Reverse[pts];
ln[{th_, a_}]:= Line[{{-0.8, a}, {th, a},  {th, 40}}]

Show[servoPlot,Epilog->{Orange,Dashed,Thick, ln[{pts[[1]],uLim}],ln[{pts[[2]],lLim}]}]

servoClamp=NonlinearStateSpaceModel[{{},With[{lLim=lLim,uLim=uLim},Which[angle≤lLim,lLim,angle≥uLim,uLim,True,angle]]},{},angle,SamplingPeriod->T]

potFit=LinearModelFit[{{0,-0.06},{5,0.06}},v,v]

Plot[potFit[v],{v,0,5},Axes->False,Frame->True,FrameLabel->{"voltage (volts)", "r\!\(\*SubscriptBox[\(\\ \), \(ref\)]\)(m)"},ImageSize->Medium,GridLines->Automatic]

pot=NonlinearStateSpaceModel[{{},{potFit[v]}},{},v,SamplingPeriod->T]

comp=StateSpaceModel[{{},{},{},{{1,-1}}},SamplingPeriod->T]

compC=StateSpaceModel[{{},{},{},{{1,-1}}}]

aSin=NonlinearStateSpaceModel[{{},Which[u<=-0.99,-(π/2),u≥0.99,(π/2),True,ArcSin[u]]},{},u,SamplingPeriod->T]

sinClamp=NonlinearStateSpaceModel[{{},Which[u<=-0.99,-0.99,u≥0.99,0.99,True,u]},{},u,SamplingPeriod->T]

μc=<|"Target"->"ArduinoUno","Inputs"->{"A1"->"Analog","A0"->"Analog"},"Outputs"->servoOut,"Timer"->0|>;

embedSpecs=Sequence[μc,port,<|"Libraries"->servoLib|>];

T=0.05;

pidData=PIDTune[ballAndBeam,"PIFD","PIDData"]

csysPID=pidData["ReferenceOutput"]

Plot[Evaluate@OutputResponse[csysPID,0.03,{t,0,10}],{t,0,10},PlotRange->All]

Eigenvalues[Normal[csysPID][[1]]]

pidc=pidData["Feedback"]

pidData["Feedforward"]

pid=ToDiscreteTimeModel[pidc,T]

controllerPID=SystemsConnectionsModel[{pot,comp,pid,aSin,servo,servoClamp, sensor},{{1,1}\[DirectedEdge]{2,1},{2,1}\[DirectedEdge]{3,1},{3,1}\[DirectedEdge]{4,1},{4,1}\[DirectedEdge]{5,1},{5,1}\[DirectedEdge]{6,1},{7,1}\[DirectedEdge]{2,2}},{{1,1},{7,1}},{{6,1}}]

Needs["MicrocontrollerKit`"]

MicrocontrollerEmbedCode[controllerPID,embedSpecs]

poles={-3+I,-3-I};
𝓀=StateFeedbackGains[ballAndBeam,poles]

estimPoles={-8,-10};
ℓ=EstimatorGains[ballAndBeam,estimPoles]

estim=SystemsModelExtract[StateOutputEstimator[ballAndBeam,ℓ],All,{1,2}]

fbc=SystemsModelSeriesConnect[estim,StateSpaceModel[{{},{},{},𝓀}]]

{a,b,c,d}=Normal[ballAndBeam];
𝓃=Inverse[ArrayFlatten[{{a,b},{c,d}}]].SparseArray[{{3,1}->1}];

ffc=StateSpaceModel[{{},{},{},𝓃[[{3}]]+𝓀.𝓃[[1;;2]]}]

csysPP=SystemsModelMerge[SystemsConnectionsModel[{ffc,compC,fbc,ballAndBeam},{{1,1}\[DirectedEdge]{2,1},{2,1}\[DirectedEdge]{4,1},{2,1}\[DirectedEdge]{3,1},{4,1}\[DirectedEdge]{3,2},{3,1}\[DirectedEdge]{2,2}},{{1,1}},{{4,1}}]]

Eigenvalues[First[Normal[csysPP]]]

Complement[%,Join[poles,estimPoles], SameTest->(Chop[#1 - #2] == 0 &)]

Plot[Evaluate@OutputResponse[csysPP,0.03,{t,0,3}],{t,0,3},PlotRange->All]

{ff,fb}=ToDiscreteTimeModel[#,T]&/@{ffc,fbc}

controllerPP=SystemsConnectionsModel[{pot,ff,comp,aSin,servo,servoClamp,sinClamp,fb,sensor},{{1,1}\[DirectedEdge]{2,1},{2,1}\[DirectedEdge]{3,1},{3,1}\[DirectedEdge]{4,1},{4,1}\[DirectedEdge]{5,1},{5,1}\[DirectedEdge]{6,1},{3,1}\[DirectedEdge]{7,1},{7,1}\[DirectedEdge]{8,1},{9,1}\[DirectedEdge]{8,2},{8,1}\[DirectedEdge]{3,2}},{{1,1},{9,1}},{{6,1}}]

MicrocontrollerEmbedCode[controllerPP,embedSpecs]

ballAndBeamInt=StateSpaceModel[{ArrayFlatten[{{0, -c},{0, a}}],
ArrayFlatten[{{0},{b}}]}]

κ=LQRegulatorGains[ballAndBeamInt,{DiagonalMatrix[{20,350,10}],{{10}}}]

eig=Eigenvalues[Normal[SystemsModelStateFeedbackConnect[ballAndBeamInt,κ]][[1]]]

ffcInt=SystemsModelSeriesConnect[StateSpaceModel[{{},{},{},-κ[[All,1;;1]]}],TransferFunctionModel[(1/s),s]]

fbcInt=SystemsModelSeriesConnect[estim,StateSpaceModel[{{},{},{},κ[[All,2;;3]]}]]

csysInt=SystemsModelMerge[SystemsConnectionsModel[{compC,ffcInt,compC,fbcInt,ballAndBeam},{{1,1}\[DirectedEdge]{2,1},{2,1}\[DirectedEdge]{3,1},{3,1}\[DirectedEdge]{5,1},{3,1}\[DirectedEdge]{4,1},{5,1}\[DirectedEdge]{4,2},{4,1}\[DirectedEdge]{3,2},{5,1}\[DirectedEdge]{1,2}},{{1,1}},{{5,1}}]]

Eigenvalues[First[Normal[csysInt]]]

Complement[%,Join[eig,estimPoles], SameTest->(Chop[#1 - #2] == 0 &)]

Plot[Evaluate@OutputResponse[csysInt,0.03,{t,0,12}],{t,0,12},PlotRange->All]

{ffInt,fbInt}=ToDiscreteTimeModel[#,T]&/@{ffcInt,fbcInt}

controllerInt=SystemsConnectionsModel[{pot,comp,ffInt,comp,aSin,servo,servoClamp,sinClamp,fbInt,sensor},{{1,1}\[DirectedEdge]{2,1},{2,1}\[DirectedEdge]{3,1},{3,1}\[DirectedEdge]{4,1},{4,1}\[DirectedEdge]{5,1},{5,1}\[DirectedEdge]{6,1},{6,1}\[DirectedEdge]{7,1},{4,1}\[DirectedEdge]{8,1},{8,1}\[DirectedEdge]{9,1},{10,1}\[DirectedEdge]{9,2},{9,1}\[DirectedEdge]{4,2}, {10,1}\[DirectedEdge]{2,2}},{{1,1},{10,1}},{{7,1}}]

MicrocontrollerEmbedCode[controllerInt, embedSpecs]

Table[OutputResponse[sys,0.01,{t,0,10}],{sys,{csysPID,csysPP,csysInt}}];
Plot[%,{t,0,10},PlotTheme->"Detailed",PlotRange->All,PlotLegends->{"PID", "pole placement","integral"},ImageSize->Medium]