DCMotorSim but from kS, kV, and kA

Technical Programming
1

I’d like to create a DCMotorSim object but all I have are SysID values for a simple motor for kS, kV, and kA. Is there a way to generate a LinearSystemSim using these values that works like DCMotorSim just without the moment of inertia and everything else.

2

Just to clarify these values were obtained with the mechanism attached to the motor so I’m just factoring all of those into the “motor” sim

3

Sample Code.

SimMotorSimple.java (1.4 KB)

4

The model for just K_v and K_a would be \frac{dx}{dt} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} where

\mathbf{x} = \begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix} \quad \mathbf{u} = \begin{bmatrix} \text{voltage} \end{bmatrix} \\ \mathbf{A} = \begin{bmatrix} 0 & 1 \\ 0 & -\frac{K_v}{K_a} \end{bmatrix} \quad \mathbf{B} = \begin{bmatrix} 0 \\ \frac{1}{K_a} \end{bmatrix}

You can use LinearSystemId.identifyPositionSystem() to make that.

The model including K_s would be \frac{dx}{dt} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} + \mathbf{c} where

\mathbf{A} = \begin{bmatrix} 0 & 1 \\ 0 & -\frac{K_v}{K_a} \end{bmatrix} \quad \mathbf{B} = \begin{bmatrix} 0 \\ \frac{1}{K_a} \end{bmatrix} \quad \mathbf{c} = \begin{bmatrix} 0 \\ -\frac{K_s}{K_a}\text{sgn}(v) \end{bmatrix}

That’s an affine system instead of a linear one tho (ignoring the cross-over point for v), so you’d have to add K_s to a custom sim.

5

I had a thought about that. So basically I would have to construct my own LinearSystemSim class.

Any changes on the Cx + Du side of things in terms of kS?

I’ll work on that. I’ll probably use LinearSystemSim as a template.

Also I’m using Subsystem and CommandScheduler to “run” the motor when I set an input. Any thing I should worry or care about doing that.

6

Well, it would be an AffineSystemSim class, but yes.

7

for the sgn(v) component I would assume I would use the current v for the system as I’m inputting voltage.

If I were to expand this to elevators and arms, subtract kG / kA and kG/Ka cos(theta) respectively?

Also do I need to do anything with the offset angle arrived from SysID for arms or is that just what was used internally by SysID

8

Yes. Simulating that properly is going to be annoying.

Yes.

It’s a measurement offset to make the model work. You can assume users apply that themselves.

9

I’m also assuming that I’ll have to discretize little c?

10

In the Discretization class would I have to alter the M and phi values to incorporate little c. If so how would that be accomplished.

11

Nevermind. I just caught my mistake in thinking. I just now noticed that c is not multiplied by u.

Therefore it shouldn’t have any bearing on the discretization process from my understanding.

12

Linear system discretization

\frac{dx}{dt} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u}
\mathbf{x}_{k+1} = e^{\mathbf{A}T}\mathbf{x}_k + \mathbf{A}^{-1}(e^{\mathbf{A}T} - \mathbf{I})\mathbf{B}\mathbf{u}_k \\ \mathbf{x}_{k+1} = \mathbf{A}_d\mathbf{x}_k + \mathbf{B}_d\mathbf{u}_k

See appendix D.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf for a more complete derivation.

Affine system discretization

\frac{dx}{dt} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} + \mathbf{c} \\ \frac{dx}{dt} = \mathbf{A}\mathbf{x} + \mathbf{B}(\mathbf{u} + \mathbf{B}^+\mathbf{c})
\mathbf{x}_{k+1} = e^{\mathbf{A}T}\mathbf{x}_k + \mathbf{A}^{-1}(e^{\mathbf{A}T} - \mathbf{I})\mathbf{B}(\mathbf{u}_k + \mathbf{B}^+\mathbf{c}_k) \\ \mathbf{x}_{k+1} = e^{\mathbf{A}T}\mathbf{x}_k + \mathbf{B}_d(\mathbf{u}_k + \mathbf{B}^+\mathbf{c}_k) \\ \mathbf{x}_{k+1} = \mathbf{A}_d\mathbf{x}_k + \mathbf{B}_d\mathbf{u}_k + \mathbf{B}_d\mathbf{B}^+ \mathbf{c}_k

where \mathbf{B}^+ is the pseudoinverse of \mathbf{B}.

You can discretize A and B like usual, then just use those in the equation above.

13

Thank you. I guess I was wrong about not needing to discretize little c. One last question (for now :grin:) Is there a pseudoinverse function in the matrix class or elsewhere in the API

14

It’s more efficient to use a linear system solver than to form the pseudoinverse directly.

15

I am beginning to suspect you guys have gone a little ways beyond high school math here…

Didn’t see this stuff myself until first year EE grad school.

1 Like
16

So using the linear system solver to find the pseudoinverse. “The .solve method” as illustrated in your code snippet. I would use Bd for m_B , what would I use for nextR and m_A?

17

Like this:

public Matrix<States, 1> calculateX(
    Matrix<States, States> A,
    Matrix<States, Inputs> B,
    Matrix<States, 1> c,
    Matrix<States, 1> x,
    Matrix<Inputs, 1> u,
    double dt) {
    var discABPair = Discretization.discretizeAB(A, B, dt);
    var discA = discABPair.getFirst();
    var discB = discABPair.getSecond();

    return discA.times(x).plus(discB.times(u)).plus(discB.times(B.solve(c)));
}
18

Thanks once again. Any way of creating a Matrix<States, N1> set to nothing for testing purposes. I can’t seem to get Matrix.mat to work for this.

19

Define nothing. Do you mean not assigning an object to it, or initializing it to the zero vector? The no-arg constructor will do the latter.

20

I got it. Almost done with both an AffineSystem and an AffineSystemSim. I used a Supplier to handle passing in a function like signum