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")