def _calcGainsVel(kv, ka, qv, effort, period):
# If acceleration for velocity control requires no effort, the feedback
# control gains approach zero. We special-case it here because numerical
# instabilities arise in LQR otherwise.
if ka < 1e-7:
return 0, 0
A = np.array([[-kv / ka]])
B = np.array([[1 / ka]])
C = np.array([[1]])
D = np.array([[0]])
sys = cnt.ss(A, B, C, D)
dsys = sys.sample(period)
# Assign Q and R matrices according to Bryson's rule [1]. The elements
# of q and r are tunable by the user.
#
# [1] 'Bryson's rule' in
# https://file.tavsys.net/control/state-space-guide.pdf
q = [qv] # units/s acceptable error
r = [effort] # V acceptable actuation effort
Q = np.diag(1.0 / np.square(q))
R = np.diag(1.0 / np.square(r))
K = frccnt.lqr(dsys, Q, R)
kp = K[0, 0]
kd = 0
return kp, kd
def _calcGains(kv, ka, qp, qv, effort, period, position_delay):
# If acceleration requires no effort, velocity becomes an input for position
# control. We choose an appropriate model in this case to avoid numerical
# instabilities in LQR.
if ka > 1e-7:
A = np.array([[0, 1], [0, -kv / ka]])
B = np.array([[0], [1 / ka]])
C = np.array([[1, 0]])
D = np.array([[0]])
q = [qp, qv] # units and units/s acceptable errors
r = [effort] # V acceptable actuation effort
else:
A = np.array([[0]])
B = np.array([[1]])
C = np.array([[1]])
D = np.array([[0]])
q = [qp] # units acceptable error
r = [qv] # units/s acceptable error
sys = cnt.ss(A, B, C, D)
dsys = sys.sample(period)
# Assign Q and R matrices according to Bryson's rule [1]. The elements
# of q and r are tunable by the user.
#
# [1] 'Bryson's rule' in
# https://file.tavsys.net/control/state-space-guide.pdf
Q = np.diag(1.0 / np.square(q))
R = np.diag(1.0 / np.square(r))
K = frccnt.lqr(dsys, Q, R)
if position_delay > 0:
# This corrects the gain to compensate for measurement delay, which
# can be quite large as a result of filtering for some motor
# controller and sensor combinations. Note that this will result in
# an overly conservative (i.e. non-optimal) gain, because we need to
# have a time-varying control gain to give the system an initial kick
# in the right direction. The state will converge to zero and the
# controller gain will converge to the steady-state one the tool outputs.
#
# See E.4.2 in
# https://file.tavsys.net/control/controls-engineering-in-frc.pdf
delay_in_seconds = position_delay / 1000 # ms -> s
K = K @ np.linalg.matrix_power(dsys.A - dsys.B @ K,
round(delay_in_seconds / period))
# With the alternate model, `kp = kv * K[0, 0]` is used because the gain
# produced by LQR is for velocity. We can use the feedforward equation
# `u = kv * v` to convert velocity to voltage. `kd = 0` because velocity
# was an input; we don't need feedback control to command it.
if ka > 1e-7:
kp = K[0, 0]
kd = K[0, 1]
else:
kp = kv * K[0, 0]
kd = 0
return kp, kd
def relinearize(self, Q_elems, R_elems, states, inputs):
from frccontrol import lqr
sysc = self.create_model(states, inputs)
sysd = sysc.sample(self.dt)
Q = np.diag(1.0 / np.square(Q_elems))
R = np.diag(1.0 / np.square(R_elems))
return lqr(sysd, Q, R)
def calculate(self, pose_desired, v_desired, omega_desired, pose, v):
error = pose_desired.relative_to(pose)
e = np.array([[error.x], [error.y], [error.theta]])
sys = self.make_model(v)
dsys = sys.sample(0.02)
K = fct.lqr(dsys, self.Q, self.R)
u = K @ e
return v_desired + u[0, 0], omega_desired + u[1, 0]
def _calcGains(kv, ka, qp, qv, effort, period):
# If acceleration requires no effort, velocity becomes an input for position
# control. We choose an appropriate model in this case to avoid numerical
# instabilities in LQR.
if ka > 1e-7:
A = np.array([[0, 1], [0, -kv / ka]])
B = np.array([[0], [1 / ka]])
C = np.array([[1, 0]])
D = np.array([[0]])
q = [qp, qv] # units and units/s acceptable errors
r = [effort] # V acceptable actuation effort
else:
A = np.array([[0]])
B = np.array([[1]])
C = np.array([[1]])
D = np.array([[0]])
q = [qp] # units acceptable error
r = [qv] # units/s acceptable error
sys = cnt.ss(A, B, C, D)
dsys = sys.sample(period)
# Assign Q and R matrices according to Bryson's rule [1]. The elements
# of q and r are tunable by the user.
#
# [1] 'Bryson's rule' in
# https://file.tavsys.net/control/state-space-guide.pdf
Q = np.diag(1.0 / np.square(q))
R = np.diag(1.0 / np.square(r))
K = frccnt.lqr(dsys, Q, R)
# With the alternate model, `kp = kv * K[0, 0]` is used because the gain
# produced by LQR is for velocity. We can use the feedforward equation
# `u = kv * v` to convert velocity to voltage. `kd = 0` because velocity
# was an input; we don't need feedback control to command it.
if ka > 1e-7:
kp = K[0, 0]
kd = K[0, 1]
else:
kp = kv * K[0, 0]
kd = 0
return kp, kd
def _calcGainsVel(kv, ka, qv, effort, period, velocity_delay):
# If acceleration for velocity control requires no effort, the feedback
# control gains approach zero. We special-case it here because numerical
# instabilities arise in LQR otherwise.
if ka < 1e-7:
return 0, 0
A = np.array([[-kv / ka]])
B = np.array([[1 / ka]])
C = np.array([[1]])
D = np.array([[0]])
sys = cnt.ss(A, B, C, D)
dsys = sys.sample(period)
# Assign Q and R matrices according to Bryson's rule [1]. The elements
# of q and r are tunable by the user.
#
# [1] 'Bryson's rule' in
# https://file.tavsys.net/control/state-space-guide.pdf
q = [qv] # units/s acceptable error
r = [effort] # V acceptable actuation effort
Q = np.diag(1.0 / np.square(q))
R = np.diag(1.0 / np.square(r))
K = frccnt.lqr(dsys, Q, R)
if velocity_delay > 0:
# This corrects the gain to compensate for measurement delay, which
# can be quite large as a result of filtering for some motor
# controller and sensor combinations. Note that this will result in
# an overly conservative (i.e. non-optimal) gain, because we need to
# have a time-varying control gain to give the system an initial kick
# in the right direction. The state will converge to zero and the
# controller gain will converge to the steady-state one the tool outputs.
#
# See E.4.2 in
# https://file.tavsys.net/control/controls-engineering-in-frc.pdf
delay_in_seconds = velocity_delay / 1000 # ms -> s
K = K @ np.linalg.matrix_power(dsys.A - dsys.B @ K,
round(delay_in_seconds / period))
kp = K[0, 0]
kd = 0
return kp, kd
def _calcGains(kv, ka, qp, qv, effort, period):
A = np.array([[0, 1], [0, -kv / ka]])
B = np.array([[0], [1 / ka]])
C = np.array([[1, 0]])
D = np.array([[0]])
sys = cnt.ss(A, B, C, D)
dsys = sys.sample(period)
# Assign Q and R matrices according to Bryson's rule [1]. The elements
# of q and r are tunable by the user.
#
# [1] 'Bryson's rule' in
# https://file.tavsys.net/control/state-space-guide.pdf
q = [qp, qv] # units and units/s acceptable errors
r = [effort] # V acceptable actuation effort
Q = np.diag(1.0 / np.square(q))
R = np.diag(1.0 / np.square(r))
K = frccnt.lqr(dsys, Q, R)
kp = K[0, 0]
kd = K[0, 1]
return kp, kd
def main():
J = 7.7500e-05
b = 8.9100e-05
Kt = 0.0184
Ke = 0.0211
R = 0.0916
L = 5.9000e-05
# fmt: off
A = np.array([[-b / J, Kt / J],
[-Ke / L, -R / L]])
B = np.array([[0],
[1 / L]])
C = np.array([[1, 0]])
D = np.array([[0]])
# fmt: on
sysc = ct.StateSpace(A, B, C, D)
dt = 0.0001
tmax = 0.025
sysd = sysc.sample(dt)
# fmt: off
Q = np.array([[1 / 20**2, 0],
[0, 1 / 40**2]])
R = np.array([[1 / 12**2]])
# fmt: on
K = fct.lqr(sysd, Q, R)
# Steady-state feedforward
tmp1 = concatenate(
(
concatenate((sysd.A - np.eye(sysd.A.shape[0]), sysd.B), axis=1),
concatenate((sysd.C, sysd.D), axis=1),
),
axis=0,
)
tmp2 = concatenate(
(np.zeros((sysd.A.shape[0], 1)), np.ones((sysd.C.shape[0], 1))), axis=0
)
NxNu = np.linalg.pinv(tmp1) * tmp2
Nx = NxNu[0 : sysd.A.shape[0], 0]
Nu = NxNu[sysd.C.shape[0] + 1 :, 0]
Kff_ss = Nu * np.linalg.pinv(Nx)
# Plant inversions
Kff_ts1 = np.linalg.inv(sysd.B.T * Q * sysd.B + R) * (sysd.B.T * Q)
Kff_ts2 = np.linalg.pinv(sysd.B)
t = np.arange(0, tmax, dt)
r = np.array([[2000 * 0.1047], [0]])
r_rec = np.zeros((2, 1, len(t)))
# No feedforward
x = np.array([[0], [0]])
x_rec = np.zeros((2, 1, len(t)))
u_rec = np.zeros((1, 1, len(t)))
# Steady-state feedforward
x_ss = np.array([[0], [0]])
x_ss_rec = np.zeros((2, 1, len(t)))
u_ss_rec = np.zeros((1, 1, len(t)))
# Plant inversion (Q and R cost)
x_ts1 = np.array([[0], [0]])
x_ts1_rec = np.zeros((2, 1, len(t)))
u_ts1_rec = np.zeros((1, 1, len(t)))
# Plant inversion (Q cost only)
x_ts2 = np.array([[0], [0]])
x_ts2_rec = np.zeros((2, 1, len(t)))
u_ts2_rec = np.zeros((1, 1, len(t)))
u_min = np.asarray(-12)
u_max = np.asarray(12)
for k in range(len(t)):
r_rec[:, :, k] = r
# Without feedforward
u = K @ (r - x)
u = np.clip(u, u_min, u_max)
x = sysd.A @ x + sysd.B @ u
x_rec[:, :, k] = x
u_rec[:, :, k] = u
# With steady-state feedforward
u_ss = K @ (r - x_ss) + Kff_ss @ r
u_ss = np.clip(u_ss, u_min, u_max)
x_ss = sysd.A @ x_ss + sysd.B @ u_ss
x_ss_rec[:, :, k] = x_ss
u_ss_rec[:, :, k] = u_ss
# Plant inversion (Q and R cost)
u_ts1 = K @ (r - x_ts1) + Kff_ts1 @ (r - sysd.A @ r)
u_ts1 = np.clip(u_ts1, u_min, u_max)
x_ts1 = sysd.A @ x_ts1 + sysd.B @ u_ts1
x_ts1_rec[:, :, k] = x_ts1
u_ts1_rec[:, :, k] = u_ts1
# Plant inversion
u_ts2 = K @ (r - x_ts2) + Kff_ts2 @ (r - sysd.A @ r)
u_ts2 = np.clip(u_ts2, u_min, u_max)
x_ts2 = sysd.A @ x_ts2 + sysd.B @ u_ts2
x_ts2_rec[:, :, k] = x_ts2
u_ts2_rec[:, :, k] = u_ts2
plt.figure(1)
plt.subplot(3, 1, 1)
plt.plot(t, r_rec[0, 0, :], label="Reference")
plt.ylabel("$\omega$ (rad/s)")
plt.plot(t, x_rec[0, 0, :], label="No FF")
plt.plot(t, x_ss_rec[0, 0, :], label="Steady-state FF")
plt.plot(t, x_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t, x_rec[1, 0, :], label="No FF")
plt.plot(t, x_ss_rec[1, 0, :], label="Steady-state FF")
plt.plot(t, x_ts1_rec[1, 0, :], label="Plant inversion (Q and R cost)")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t, u_rec[0, 0, :], label="No FF")
plt.plot(t, u_ss_rec[0, 0, :], label="Steady-state FF")
plt.plot(t, u_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("case_study_ss_ff1")
plt.figure(2)
plt.subplot(3, 1, 1)
plt.plot(t, r_rec[0, 0, :], label="Reference")
plt.ylabel("$\omega$ (rad/s)")
plt.plot(t, x_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.plot(t, x_ts2_rec[0, 0, :], label="Plant inversion (Q cost only)")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t, x_ts1_rec[1, 0, :], label="Plant inversion (Q and R cost)")
plt.plot(t, x_ts2_rec[1, 0, :], label="Plant inversion (Q cost only)")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t, u_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.plot(t, u_ts2_rec[0, 0, :], label="Plant inversion (Q cost only)")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("case_study_ss_ff2")
else:
plt.show()
def main():
J = 7.7500e-05
b = 8.9100e-05
Kt = 0.0184
Ke = 0.0211
R = 0.0916
L = 5.9000e-05
# fmt: off
A = np.array([[-b / J, Kt / J], [-Ke / L, -R / L]])
B = np.array([[0], [1 / L]])
C = np.array([[1, 0]])
D = np.array([[0]])
# fmt: on
sysc = cnt.StateSpace(A, B, C, D)
dt = 0.0001
tmax = 0.025
sysd = sysc.sample(dt)
# fmt: off
Q = np.array([[1 / 20**2, 0], [0, 1 / 40**2]])
R = np.array([[1 / 12**2]])
# fmt: on
K_pp1 = cnt.place(sysd.A, sysd.B, [0.1, 0.9])
K_pp2 = cnt.place(sysd.A, sysd.B, [0.1, 0.8])
K_lqr = frccnt.lqr(sysd, Q, R)
t = np.arange(0, tmax, dt)
r = np.array([[2000 * 0.1047], [0]])
r_rec = np.zeros((2, 1, len(t)))
# Pole placement 1
x_pp1 = np.array([[0], [0]])
x_pp1_rec = np.zeros((2, 1, len(t)))
u_pp1_rec = np.zeros((1, 1, len(t)))
# Pole placement 2
x_pp2 = np.array([[0], [0]])
x_pp2_rec = np.zeros((2, 1, len(t)))
u_pp2_rec = np.zeros((1, 1, len(t)))
# LQR
x_lqr = np.array([[0], [0]])
x_lqr_rec = np.zeros((2, 1, len(t)))
u_lqr_rec = np.zeros((1, 1, len(t)))
u_min = np.asarray(-12)
u_max = np.asarray(12)
for k in range(len(t)):
# Pole placement 1
u_pp1 = K_pp1 @ (r - x_pp1)
# Pole placement 2
u_pp2 = K_pp2 @ (r - x_pp2)
# LQR
u_lqr = K_lqr @ (r - x_lqr)
u_pp1 = np.clip(u_pp1, u_min, u_max)
x_pp1 = sysd.A @ x_pp1 + sysd.B @ u_pp1
u_pp2 = np.clip(u_pp2, u_min, u_max)
x_pp2 = sysd.A @ x_pp2 + sysd.B @ u_pp2
u_lqr = np.clip(u_lqr, u_min, u_max)
x_lqr = sysd.A @ x_lqr + sysd.B @ u_lqr
r_rec[:, :, k] = r
x_pp1_rec[:, :, k] = x_pp1
u_pp1_rec[:, :, k] = u_pp1
x_pp2_rec[:, :, k] = x_pp2
u_pp2_rec[:, :, k] = u_pp2
x_lqr_rec[:, :, k] = x_lqr
u_lqr_rec[:, :, k] = u_lqr
plt.figure(1)
plt.subplot(3, 1, 1)
plt.plot(t, r_rec[0, 0, :], label="Reference")
plt.ylabel("$\omega$ (rad/s)")
plt.plot(t,
x_pp1_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
plt.plot(t,
x_pp2_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
plt.plot(t, x_lqr_rec[0, 0, :], label="LQR")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t,
x_pp1_rec[1, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
plt.plot(t,
x_pp2_rec[1, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
plt.plot(t, x_lqr_rec[1, 0, :], label="LQR")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t,
u_pp1_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
plt.plot(t,
u_pp2_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
plt.plot(t, u_lqr_rec[0, 0, :], label="LQR")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latexutils.savefig("case_study_pp_lqr")
else:
plt.show()
def main():
J = 7.7500e-05
b = 8.9100e-05
Kt = 0.0184
Ke = 0.0211
R = 0.0916
L = 5.9000e-05
# fmt: off
A = np.array([[-b / J, Kt / J], [-Ke / L, -R / L]])
B = np.array([[0], [1 / L]])
C = np.array([[1, 0]])
D = np.array([[0]])
# fmt: on
sysc = ct.StateSpace(A, B, C, D)
dt = 0.0001
tmax = 0.025
sysd = sysc.sample(dt)
# fmt: off
Q = np.array([[1 / 20**2, 0], [0, 1 / 40**2]])
R = np.array([[1 / 12**2]])
# fmt: on
K = fct.lqr(sysd, Q, R)
# Plant inversions
Kff_ts1 = np.linalg.pinv(sysd.B)
Kff_ts2 = np.linalg.inv(sysd.B.T * Q * sysd.B + R) * (sysd.B.T * Q)
t = np.arange(0, tmax, dt)
r = np.array([[2000 * 0.1047], [0]])
r_rec = np.zeros((2, 1, len(t)))
# No feedforward
x = np.array([[0], [0]])
x_rec = np.zeros((2, 1, len(t)))
u_rec = np.zeros((1, 1, len(t)))
# Plant inversion
x_ts1 = np.array([[0], [0]])
x_ts1_rec = np.zeros((2, 1, len(t)))
u_ts1_rec = np.zeros((1, 1, len(t)))
# Plant inversion (Q and R cost)
x_ts2 = np.array([[0], [0]])
x_ts2_rec = np.zeros((2, 1, len(t)))
u_ts2_rec = np.zeros((1, 1, len(t)))
u_min = np.asarray(-12)
u_max = np.asarray(12)
for k in range(len(t)):
r_rec[:, :, k] = r
# Without feedforward
u = K @ (r - x)
u = np.clip(u, u_min, u_max)
x = sysd.A @ x + sysd.B @ u
x_rec[:, :, k] = x
u_rec[:, :, k] = u
# Plant inversion
u_ts1 = K @ (r - x_ts1) + Kff_ts1 @ (r - sysd.A @ r)
u_ts1 = np.clip(u_ts1, u_min, u_max)
x_ts1 = sysd.A @ x_ts1 + sysd.B @ u_ts1
x_ts1_rec[:, :, k] = x_ts1
u_ts1_rec[:, :, k] = u_ts1
plt.figure(1)
plt.subplot(3, 1, 1)
plt.plot(t, r_rec[0, 0, :], label="Reference")
plt.ylabel("$\omega$ (rad/s)")
plt.plot(t, x_rec[0, 0, :], label="No feedforward")
plt.plot(t, x_ts1_rec[0, 0, :], label="Plant inversion")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t, x_rec[1, 0, :], label="No feedforward")
plt.plot(t, x_ts1_rec[1, 0, :], label="Plant inversion")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t, u_rec[0, 0, :], label="No feedforward")
plt.plot(t, u_ts1_rec[0, 0, :], label="Plant inversion")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("case_study_ff")
else:
plt.show()
def A(n):
return np.array([[0, 0, 0], [0, 0, n], [0, 0, 0]])
B = np.array([[1, 0], [0, 0], [0, 1]])
A1 = A(1e-7)
A2 = A(1)
states = 3
inputs = 2
outputs = 1
C = np.zeros((outputs, states))
D = np.zeros((outputs, inputs))
sys1 = cnt.StateSpace(A1, B, C, D)
sys2 = cnt.StateSpace(A2, B, C, D)
sys1d = sys1.sample(1.0 / 50.0)
sys2d = sys2.sample(1.0 / 50.0)
# python-control gives LQR gains that are far too high for the inputs
# Probably a bug in SLICOT, so just use frccontrol for the calculations
print(sys1d.A)
print(Q)
K1 = frccnt.lqr(sys1d, Q, R)
K2 = frccnt.lqr(sys2d, Q, R)
print(f"kx={K1[0, 0]}, ky0={K1[1, 1]}")
print(f"ky1={K2[1, 1]}, ktheta={K2[1, 2]}")
def calcGains():
v = 1e-7
# States: x, y, theta
# Inputs: v, omega
A = np.array([[0, 0, 0], [0, 0, v], [0, 0, 0]])
B = np.array([[1, 0], [0, 0], [0, 1]])
C = np.array([[0, 0, 1]])
D = np.array([[0, 0]])
qX = 0.12 # allowable linear error
qY = 0.2 # allowable cross track error
qTheta = math.radians(90) # allowable heading error
vMax = 0.61 * 2.5 # excursion from reference velocity
wMax = math.radians(170.0) # max turn excursion from reference
print(
"Calculating cost matrix for LTV Unicycle with costs qX %s qY %s qTheta %s vMax %s wMax %s\n"
% (qX, qY, qTheta, vMax, wMax))
Q = __make_cost_matrix([qX, qY, qTheta])
R = __make_cost_matrix([vMax, wMax])
sys = ct.StateSpace(A, B, C, D, remove_useless=False)
sysd = sys.sample(1.0 / 50.0)
K_0 = frccnt.lqr(sysd, Q, R)
v = 1.0
# States: x, y, theta
# Inputs: v, omega
A = np.array([[0, 0, 0], [0, 0, v], [0, 0, 0]])
Q = __make_cost_matrix([qX, qY, qTheta])
R = __make_cost_matrix([vMax, wMax])
sys = ct.StateSpace(A, B, C, D, remove_useless=False)
sysd = sys.sample(1.0 / 50.0)
K_1 = frccnt.lqr(sysd, Q, R)
# print("\nK0 = \n%s\n " % K_0)
# print("K1 = \n%s\n" % K_1)
print("kx = %s" % K_0[1 - 1, 1 - 1])
print("ky_0 = %s" % K_0[2 - 1, 2 - 1])
print("ky_1 = %s" % K_1[2 - 1, 2 - 1])
print("kTheta = %s\n" % K_1[2 - 1, 3 - 1])
gains = ("%s, %s, %s, %s" % (K_0[1 - 1, 1 - 1], K_0[2 - 1, 2 - 1],
K_1[2 - 1, 2 - 1], K_1[2 - 1, 3 - 1]))
# print(gains)
# Save it to a file
fileName = "GeneratedLTVUnicycleGains.kt"
file = open("src/generated/kotlin/statespace/" + fileName, "w")
file.write(
"package statespace\n\n" +
"import org.team5940.pantry.lib.statespace.LTVUnicycleController\n\n" +
"val generatedLTVUnicycleController get() = LTVUnicycleController(\n" +
" %s\n" % gains + ")\n")
def calcGains(isHighGear):
qX = 0.12 # allowable linear error
qY = 0.2 # allowable cross track error
qTheta = math.radians(90) # allowable heading error
vMax = 5.0 * 0.3048 # allowable velocity error? (ft/sec to meters per sec)
voltMax = 5.0 # max control effort (from trajectory feedforward?)
# period of the controller
period = 1.0 / 100.0
print(
"Calculating cost matrix for LTV diff drive controller with costs qX %s qY %s qTheta %s vMax %s at %s volts \n"
% (qX, qY, qTheta, vMax, voltMax))
if (isHighGear):
G = Ghigh
gear = "High"
else:
G = Glow
gear = "Low"
C1 = -G**2 * motor.Kt / (motor.Kv * motor.R * r**2)
C2 = G * motor.Kt / (motor.R * r)
C3 = -G**2 * motor.Kt / (motor.Kv * motor.R * r**2)
C4 = G * motor.Kt / (motor.R * r)
Q = __make_cost_matrix([qX, qY, qTheta, vMax, vMax])
R = __make_cost_matrix([voltMax, voltMax])
C = np.array([[0, 0, 1, 0, 0], [0, 0, 0, 1, 0], [0, 0, 0, 0, 1]])
D = np.array([[0, 0], [0, 0], [0, 0]])
v = 1e-7
A = calcA(v, isHighGear, C1, C3)
B = calcB(v, isHighGear, C2, C4)
sys = ct.StateSpace(A, B, C, D, remove_useless=False)
sysd = sys.sample(period)
K0 = frccnt.lqr(sysd, Q, R)
v = 1.0
A = calcA(v, isHighGear, C1, C3)
B = calcB(v, isHighGear, C2, C4)
sys = ct.StateSpace(A, B, C, D, remove_useless=False)
sysd = sys.sample(period)
K1 = frccnt.lqr(sysd, Q, R)
kx = K0[0, 0]
ky_0 = K0[0, 1]
kvPlus_0 = K0[0, 3]
kVMinus_0 = K0[1, 3]
ky_1 = K1[0, 1]
kTheta_1 = K1[0, 2]
kVPlus_1 = K1[0, 3]
gains = str("%s, %s, %s, %s,\n %s, %s, %s" %
(kx, ky_0, kvPlus_0, kVMinus_0, ky_1, kTheta_1, kVPlus_1))
# print((gains))
print("LTV Diff Drive CTE Controller gains: \n %s" % gains)
# Save it to a file
fileName = "GeneratedLTVDiffDriveController.kt"
file = open("src/generated/kotlin/statespace/" + fileName, "w")
file.write(
"package statespace\n\n" +
"import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics\n"
+
"import org.team5940.pantry.lib.statespace.LTVDiffDriveController\n\n"
+ "val generatedLTVDiffDriveController" + gear +
"Gear get() = LTVDiffDriveController(\n " + gains +
",\n DifferentialDriveKinematics(" + str(rb) + ")" + ")\n")
