Help with state-space controller?

[Spoam]

I'm trying to write a full state feedback controller for a DC motor (with a load attached). I derived the same model as 971 with voltage as input, displacement as output and [position, velocity] as the state. I used 971's eigenvalues and model constants because I'm a newbie and neither know how to pick poles myself, nor how to pick weighting factors to do LQR. The simulation I wrote seems to converge fine for a desired position (e.g. [1,0]). However, for a velocity it converges to about 65% of the velocity I want (e.g. a desired state of [x_hat(k), 1] asymptotically approaches [x_hat(k), .65] instead) (and what it converges to depends on the poles, which seems bizarre to me; my intuition tells me <1 eigenvalues should stabilize to 0 not something arbitrary like .35). Anyone know why?

Here's the simulation code I wrote in matlab

Kt = .498192/85;
R = 12/85/2;
Kv = (17800/60*2*3.141592)/(12 - 12/85/2*1.4);
J = .0032;
G = 11/34;
%^971's values

%time step
dt = .1;

%not affected by anything but input and inertia
A = [0      1
     0 -Kt/(Kv*R*J*G^2)];
%can directly control acceleration
B = [   0
     Kt/(R*J*G)];
%can observe position
C = [1 0];

%put A and B in discrete time space
A_d = expm(A*dt);
B_d = pinv(A)*(A_d - eye(size(A_d)))*B;

%I have no idea how to pick poles
%so here are 971's
P_K = [.6 .981];
P_L = [.45-.07i,.45+.07i];
K = place(A_d,B_d,P_K);
L = place(A_d.',C.',P_L).';

%initialize
x = [0;0];
y = C*x;
x_hat = [0;0];

t=0;
while t<100;
    Rs = [x_hat(1);1]; 

    u = K*(Rs-x_hat);
    u(u>12) = 12;
    u(u<-12) = -12;

    x_hat = A_d*x_hat +B_d*u + L*(y - C*x_hat);
    x = A_d*x + B_d*u;
    y = C*x;

    t = t+dt;
end;

Also, I imagine to use it as a real controller I would just replace "C*x" with an encoder/sensor value and feed u to the motor controller, correct?
If the code is completely wrong somewhere please tell me

[James Kuszmaul]

Originally Posted by Spoam

However, for a velocity it converges to about 65% of the velocity I want (e.g. a desired state of [x_hat(k), 1] asymptotically approaches [x_hat(k), .65] instead) (and what it converges to depends on the poles, which seems bizarre to me; my intuition tells me <1 eigenvalues should stabilize to 0 not something arbitrary like .35). Anyone know why?

The reason for this is that, in order to sustain a constant velocity, the motor will require a non-zero voltage, but when you reach your goal velocity, R - x_hat = 0 and so the output voltage is zero and so the motor will not actually achieve that velocity. In order to handle this, you need to add a feed-forward value to U which is proportional to the goal velocity.
Another trick that 971 has used in the past is, instead of always updating the position goal to be the same as the current position, instead, on each timestep, update the position goal by the velocity goal * dt and make sure that the position goal never runs off too far from the current position.

%put A and B in discrete time space
A_d = expm(A*dt);
B_d = pinv(A)*(A_d - eye(size(A_d)))*B;

Since you're using matlab, you may as well use the provided c2d function; python has no such function, hence why 971 implemented it themselves.

Also, I imagine to use it as a real controller I would just replace "C*x" with an encoder/sensor value and feed u to the motor controller, correct?

That is correct.

If the code is completely wrong somewhere please tell me

Nothing strikes me as wrong; I haven't tried running your code myself though.

Good luck getting this working; I had a lot of fun learning this stuff in High School.