sketchBookDir=FileNameJoin[{$HomeDirectory,"Documents","Arduino","libraries"}];

libs=Table[FileNameJoin[{sketchBookDir,d}],{d,{"QTRSensors","ZumoReflectanceSensorArray","Pushbutton","ZumoMotors"}}];

DirectoryQ/@libs

eq=θ'[t]==((r ((Subscript[Φ, r])[t]-(Subscript[Φ, l])[t]))/(2 yd));

controlMixerRules={(Subscript[Φ, l])[t]-> Φ[t]-Δ[t],(Subscript[Φ, r])[t]-> Φ[t]+Δ[t]};

zumoModel=eq/.controlMixerRules

ssm=StateSpaceModel[zumoModel,θ[t], Δ[t],θ[t],t]/.{r->(1.8/100),yd-> (4/100)}

{a,b,c,d}=Normal[ssm];

ssmI=StateSpaceModel[{ArrayFlatten[{{0, -1},{0,a}}],ArrayFlatten[{{0},{b}}],{{1,0}}}]

ControllableModelQ[ssmI]

𝓀=StateFeedbackGains[ssmI,{-14 ,-16 }]

ObservableModelQ[ssmI]

ℓ=EstimatorGains[ssmI,{-700,-800}]

ℯ𝓇=EstimatorRegulator[ssmI, {ℓ, 𝓀}]

𝒾𝓃𝓉=TransferFunctionModel[(1/s),s]

cascade[s__] := Fold[SystemsModelSeriesConnect, First[{s}], Rest[{s}]]

lg =cascade[𝒾𝓃𝓉,ℯ𝓇,(TransferFunctionModelFull[-1, , , ]),ssm]

csys=SystemsModelFeedbackConnect[lg]
Eigenvalues[Normal[%][[1]]]

OutputResponse[csys,15°(UnitStep[t]-UnitStep[t-0.5]),{t,0,2}];
Plot[%/Degree,{t,0,2},PlotRange->All]

𝒸𝓃𝓋 =(TransferFunctionModelFull[(30/π), , , ]);

𝒸=cascade[(TransferFunctionModelFull[-1, , , ]),𝒾𝓃𝓉,ℯ𝓇,(TransferFunctionModelFull[-1, , , ]),𝒸𝓃𝓋]

𝒸𝒹=ToDiscreteTimeModel[𝒸,0.001]

arg={Abs[u]<1 °, 325,1°≤ Abs[u]<2 °, 300,2 °≤ Abs[u]<5°, 150,5°≤ Abs[u]<25°, 100,True, 35}//N;
(Subscript[𝓈𝓅, 1])=NonlinearStateSpaceModel[{{},Which@@arg},{},u,SamplingPeriod->0.001]

ParametricPlot[{(u/Degree),Which@@arg},{u,-30 °,30°},AspectRatio->1,AxesOrigin->{0,0},PlotRange->All]

(Subscript[𝓈𝓅, 2])=cascade[StateSpaceModel[y[k]==(1.0/3) (u[k-2]+u[k-1]+u[k]),y[k],u[k],y[k],k,SamplingPeriod->0.001],ToDiscreteTimeModel[TransferFunctionModel[(1/(sbar+1)),sbar],0.001]]

sensorCalibFile=FileNameJoin[{sketchBookDir,"ZumoReflectanceSensorArray","examples","SensorCalibration","SensorCalibration.ino"}];

FileExistsQ[sensorCalibFile]

(*RunProcess[$SystemShell,All,"open "<>sensorCalibFile] *)(* non-Mac?*)

sensorData=;

sensorFit=LinearModelFit[sensorData,r,r]

Show[ListLinePlot[Sort[sensorData],PlotStyle->Dashed,FrameLabel->{"Reading","Angle (rad)"}],Plot[sensorFit[r],{r,0,5000}],Frame->True,ImageSize->Medium]

𝓇𝓈=NonlinearStateSpaceModel[{{},sensorFit[r]},{},r,SamplingPeriod->0.001]

controller=SystemsConnectionsModel[{𝓇𝓈,𝒸𝒹,(Subscript[𝓈𝓅, 1]),(Subscript[𝓈𝓅, 2])},{{1,1}\[DirectedEdge]{2,1},{1,1}\[DirectedEdge]{3,1},{3,1}\[DirectedEdge]{4,1}},{{1,1}},{{2,1},{4,1}}]

FileNameTake /@libs

l={"unsigned int sensors[6]","reflectanceSensors.readLine(sensors)"};
ics=<|"Type"->"ExternalLibrary","Loop"->l|>;

l={"int sp","motors.setSpeeds(sp-spd, sp+spd)"};ocs={<|"Type"->"ExternalLibrary","Loop"->{"int spd"}|>,<|"Type"->"ExternalLibrary","Loop"->l|>};

incls={"<QTRSensors.h>", "<ZumoReflectanceSensorArray.h>","<ZumoMotors.h>","<Pushbutton.h>"};

utils={"ZumoReflectanceSensorArray reflectanceSensors", "ZumoMotors motors", "Pushbutton button(ZUMO_BUTTON)"};

Needs["SymbolicC`"]

calib={CDeclare["int", "i"],CFor[CAssign["i",0],COperator[Less, {"i", 80}],COperator[Increment,"i"],{CIf["(i > 10 && i <= 30) || (i > 50 && i <= 70)","motors.setSpeeds(-200,200)","motors.setSpeeds(200,-200)"],"reflectanceSensors.calibrate()","delay(20)"}],"motors.setSpeeds(0,0)"};inits={"reflectanceSensors.init()","button.waitForButton()", calib};

ToCCodeString[inits]

Needs["MicrocontrollerKit`"]

MicrocontrollerEmbedCode[controller,<|"Target"->"ArduinoUno","Inputs"->ics,"Outputs"->ocs,"IncludeFiles"-> incls,"Utilities"-> utils,"Initializations"->inits,"Timer"->None|>,"/dev/cu.usbmodem14101",<|"Libraries"-> libs|>]