Wolfram Language Microcontroller Kit: 
Line Following Zumo Robot

Suba Thomas
Wolfram Research, Inc.

Join the Conversation #WolframTechConf

Mathematical model

(Subscript[Φ, l])[t]==0  ⟹  2 yd (Subscript[θ, r])'[t] ==   r (Subscript[Φ, r])[t] 
(Subscript[Φ, r])[t]==0  ⟹  2 yd (Subscript[θ, l])'[t] == -r (Subscript[Φ, l])[t] 
                             θ'[t]==(Subscript[θ, r])'[t]+(Subscript[θ, l])'[t]

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

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

zumoModel=eq/.controlMixerRules

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

Differential input controller design

Design

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

ssmI=StateSpaceModel[{ArrayFlatten[{{0, 1},{0,a}}],ArrayFlatten[{{0},{b}}],{{1,0}}},SystemsModelLabels->{"Δ","(Subscript[θ, I])",{"(Subscript[θ, I])","θ"}}]

ControllableModelQ[ssmI]

ObservableModelQ[ssmI]

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

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

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

Simulation

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

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

csys1=SystemsModelFeedbackConnect[lg] (* θr==0 -> θ *)

csys2=SystemsModelFeedbackConnect[(TransferFunctionModelFull[1, , , ]),lg] (* line -> θ *)

Eigenvalues[Normal[csys1][[1]]]
Eigenvalues[Normal[csys2][[1]]]
Join[rpoles,epoles]//Sort

BodePlot[{csys1,csys2},ImageSize->Medium,GridLines->Automatic,PlotLegends->{"csys1 (reference)","csys2 (disturbance)"}]

Manipulate[With[{inp=θr(UnitStep[t]-UnitStep[t-0.5])},
Plot[Evaluate[{inp,OutputResponse[csys1,inp,{t,0,2}]}],{t,0,2},Exclusions->None]],{{θr,0},-10,10},AutoAction->True]

Manipulate[With[{inp=θd},
Plot[Evaluate[{inp,OutputResponse[csys2,inp,{t,0,2}]}],{t,0,2},Exclusions->None]],{{θd,0},-10,10},AutoAction->True]

Manipulate[Plot[Evaluate[StateResponse[{csys1,{0,0,0,θi}},0,{t,0,0.8}]],{t,0,0.8},PlotRange->All,PlotLegends->Automatic],{{θi,0},-10,10},AutoAction->True]

Assembly

𝒸𝓃𝓋=TransferFunctionModel[QuantityMagnitude@WolframAlpha["rad/s to rpm","WolframResult"],s](* 30/π*)

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

𝒸𝒹=ToDiscreteTimeModel[𝒸,0.001]

Speed controller design

arg={
Abs[θ]<1 °, 325,
1°≤ Abs[θ]<2 °, 300,
2 °≤ Abs[θ]<5°, 150,
5°≤ Abs[θ]<25°, 100,
True, 35}//N;

ParametricPlot[{(θ/Degree),Which@@arg},{θ,-30 °,30°},PlotRange->All,AspectRatio->1,AxesLabel->{θ,"base speed"}]

(Subscript[𝓈𝓅, 1])=NonlinearStateSpaceModel[{{},Which@@arg},{},θ,SamplingPeriod->0.001]

(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]]

Complete controller

Summary

IR sensors model

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

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

sensorData=;

sensorDataPlot=ListPlot[sensorData,FrameLabel->{"Reading","Angle (rad)"},PlotTheme->"Detailed"]

sensorFit=LinearModelFit[sensorData,r,r]

Show[sensorDataPlot,Plot[sensorFit[r],{r,0,5000}],Frame->True,ImageSize->Medium]

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

Assembly

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

I/O specifications and controller deployment

Overview

Needs["MicrocontrollerKit`"]

?MicrocontrollerEmbedCode

Input and output channels

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

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

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]

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

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

μc=<|"Target"->"ArduinoUno","IncludeFiles"-> incls,"Utilities"-> utils,"Initializations"->inits,"Inputs"->ics,"Outputs"->ocs,"Timer"->None|>;

Deployment

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

MicrocontrollerEmbedCode[controller,μc,"/dev/cu.usbmodem1411",<|"Libraries"->libs|>]

Concluding remarks

Designed and deployed a controller to a target from the WL

Although the presentation was linear, the actual design and implementation is an iterative process

Deploy any systems model

List of supported microcontrollers: EntityList[“MicrocontrollerType”]

RandomEntity["MicrocontrollerTarget",10]

Documentation