6
$\begingroup$

I've been using MicrocontrollerKit for a while now to generate code for little projects, inverse pendulums, filters, etc. Generally it works well.

I am now currently trying to design a LQI (Linear quadratic integrator) Or atleast trying to understand them and test on an already working system with a steady state error.

The design and simulation seems to work well, so now it comes to generating the code and trying it out.

lineargains = {26.8592, 2.85176, -0.0189932};
controlforce = -lineargains.{\[Theta][t] - \[Pi], \[Theta]'[t], \[Phi]'[t]};


integrator = TransferFunctionModel[ki/z, z]
ssm = NonlinearStateSpaceModel[{{}, {controlforce}}, {}, {\[Theta][t], \[Theta]'[t], \[Phi]'[t]}]

lqid = SystemsConnectionsModel[{ToDiscreteTimeModel[integrator /. ki -> 56, 0.001], ToDiscreteTimeModel[ssm, 0.001]}, {{1, 1} -> {2, 1}}, {{1, 1}, {2,2}, {2, 3}}, {2, 1}] // SystemsModelMerge


\[ScriptCapitalM]1 = MicrocontrollerEmbedCode[lqid, <|"Target" -> "ArduinoUno", "Inputs" -> {"A0" -> "Analog", "A1" -> "Analog", "A2" -> "Analog"},"Outputs" -> {"Serial"}|>, <|"ConnectionPort" -> None|>, <|"Language" -> "Wiring"|>]

\[ScriptCapitalM]1["SourceCode"]

At this point, I believe anyways, I have properly set up the system I want.

What I don't understand, is the generated Wiring code and how it applies to the discrete LQI controller. Here is a snippet to the relevant parts, and a pastebin for the full codes.

double u[3] = {0, 0, 0};
double y[1] = {84.38063590995105};
void update_nssm(double *u, double *y)
{
static double x[1] = {0};
double x1[1];
y[0] = 84.38063590995105 - 0.22561720114594416*x[0] - 0.11280860057297208*u[0] - 2.8517595773639597*u[1] + 0.018993162921974713*u[2];
x1[0] = x[0] + u[0];
x[0] = x1[0];
}
void loop()
{
double uu0 = read_adc(0);
double uu1 = read_adc(1);
double uu2 = read_adc(2);
u[0] = uu0;
u[1] = uu1;
u[2] = uu2;
update_nssm(u, y);
Serial.write(19);
Serial.print(y[0], 2);
Serial.write(17);
delay(1);
}

There is a mysterious ~84 within the code for y[1] that is generated and never called...and within update_nssm() the state-space controller itself an 84 that I can't imagine would ever result in a value that makes sense. (the controller should create a signal that would be between -2 and 2 to control the system.

This also never appears when generating the code for the more simple LQR controller.

As seen here:

\[ScriptCapitalM] = MicrocontrollerEmbedCode[ToDiscreteTimeModel[ssm, 0.001], <|"Target" -> "ArduinoUno", "Inputs" -> {"A0" -> "Analog", "A1" -> "Analog", "A2" -> "Analog"},"Outputs" -> {"Serial"}|>, <|"ConnectionPort" -> None|>, <|"Language" -> "Wiring"|>]\[ScriptCapitalM]["SourceCode"]

And the it's code:

void update_nssm(double *u, double *y)
{
y[0] = -26.8591906126124*(-M_PI + u[0]) - 2.8517595773639597*u[1] + 0.018993162921974713*u[2];
}

When comparing to the two, one could assume that the 84~ should be some kind of subtraction from $\pi$...however thus far, I have never seem Mathematica output $\pi$ in anything but radians, as such I can't understand if the generated code is anything but correct, or how this code is generated from the given lqid StateSpaceModel.

In the simpler LQR case, I can directly read and understand what was generated (and have used successfully), however in the lqid this doesn't seem to add up.

How is MicrocontrollerEmbedCode generating this code?

I fully admit I may be the one making the mistakes here...in which case, what would be the proper way to generate such a controller?

$\endgroup$
  • $\begingroup$ Happy to learn that the MicrocontrollerKit is useful to you. Thanks for the feedback. $\endgroup$ – Suba Thomas Jan 27 at 0:44
6
$\begingroup$

The value 84.38063590995105=-26.8592*π is a residue that pops up if the equilibrium value is not set correctly.

There are a few things that need to be changed to straighten things out.

You need to specify ssm with the correct equilibrium value of $\pi$ for $\theta$.

ssm = NonlinearStateSpaceModel[{{}, {controlforce}}, {}, {{θ[t], N@π}, θ'[t], ϕ'[t]}]

That way when $\theta$ is $\pi$, controlforce is 0.

(The N@π is needed due to a bug that fails to generate the correct code for π.)

Next, if you use SystemsModelMerge to merge the connections the equilibrium value gets blown away. SystemsModelMerge can preserve the equilibrium values of states and the net inputs and outputs, but equilibrium values in any intermediate connections is lost in the merge.

To prevent that you need to keep it in the unreduced state.

lqid = SystemsConnectionsModel[{ToDiscreteTimeModel[integrator /. ki -> 56, 0.001], 
   ToDiscreteTimeModel[ssm, 0.001]}, {{1, 1} -> {2, 1}}, {{1, 1}, {2,2}, {2, 3}}, {2, 1}];

With these changes the relevant generated code will be as follows and when u[0](which is u_1[0]) is π, y[0] will also be 0.

enter image description here

$\endgroup$
  • $\begingroup$ Thank you for the quick answer and solution, The solution works and i will try the generated code on the real thing when i get the chance! $\endgroup$ – morbo Jan 28 at 10:49
  • $\begingroup$ NP. In my earlier answer I incorrectly said that there was a bug in SystemsModelMerge. I have updated the answer to correct that. $\endgroup$ – Suba Thomas Jan 28 at 15:32

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.