def make_gains():
# x = | Angle |
# | Angular velocity |
# u = voltage
# y = encoder
name = 'gains'
# Parameters
moment_inertia = 4.0 * (.03**2) / 2.0 + 0.93 * (3.5 * 0.0254)**2.0 / 2.0
gear_ratio = 1.0 / 4.0
efficiency = .91
# motor characteristics
free_speed = 18700. / 60.0 * 6.28
free_current = .67
stall_torque = .71
stall_current = 134.
resistance = 12. / stall_current
torque_constant = stall_torque / stall_current
velocity_constant = (12. - free_current * resistance) / free_speed
num_motors = 2.0
sensor_ratio = 1.0
# back emf torque
emf = -(torque_constant * velocity_constant) / (
resistance * gear_ratio**2. / num_motors)
# motor torque
mtq = efficiency * torque_constant / (gear_ratio * resistance / num_motors)
# rotational acceleration
t2a = 1. / moment_inertia
A_c = np.asmatrix([[0., 1.], [0., t2a * emf]])
B_c = np.asmatrix([[0.], [t2a * mtq]])
C = np.asmatrix([[sensor_ratio, 0.]])
# Controller weighting
Q_controller = np.asmatrix([[0., 0.], [0., 5e-1]])
R_controller = np.asmatrix([[1.]])
# Noise
Q_noise = np.asmatrix([[1e-2, 0.], [0., 1e1]])
R_noise = np.asmatrix([[0.1]])
Q_ff = np.asmatrix([[0., 0.], [0., 1.]])
A_d, B_d, Q_d, R_d = c2d(A_c, B_c, dt, Q_noise, R_noise)
K = clqr(A_c, B_c, Q_controller, R_controller)
Kff = feedforwards(A_d, B_d, Q_ff)
L = dkalman(A_d, C, Q_d, R_d)
gains = StateSpaceGains(name, dt, A_d, B_d, C, None, Q_d, R_noise, K, Kff,
L)
gains.A_c = A_c
gains.B_c = B_c
gains.Q_c = Q_noise
return gains
def make_gains(second_stage, has_cube, subname='gains'):
# x = | Linear Height |
# | Linear Velocity |
# u = voltage
# y = encoder
name = subname
mass_carriage = 6.0
if second_stage:
mass_carriage += 2.5
if has_cube:
mass_carriage += 1.59
# Parameters
r = (1.0 + 1.0 / 16.0) * 0.0254
J = 0.68 * r**2 + mass_carriage * r**2
G = (12.0 / 100.0) * (14.0 / 30.0)
eff = .8
w_free = 18730. / 60. * 6.28
I_free = .7
T_stall = 0.71
I_stall = 134.
R = 12. / I_stall
Kt = T_stall / I_stall
Kv = (12. - I_free * R) / w_free
num_motors = 2.
sensor_ratio = 1.
A_c = np.asmatrix([
[0., 1.0],
[0., -Kt * Kv / (J * G * G * R)],
])
B_c = np.asmatrix([[0.], [Kt * r / (J * G * R)]])
C = np.asmatrix([[sensor_ratio, 0.]])
# Noise
Q_noise = np.asmatrix([[0., 0.], [0., 0.]])
R_noise = np.asmatrix([[1e-5]])
Q_ff = np.asmatrix([[0., 0.], [0., 1.]])
A_d, B_d, Q_d, R_d = c2d(A_c, B_c, dt, Q_noise, R_noise)
K = place(A_c, B_c, [-10.0, -7.0])
Kff = feedforwards(A_d, B_d, Q_ff)
L = dkalman(A_d, C, Q_d, R_d)
gains = StateSpaceGains(name, dt, A_d, B_d, C, None, Q_d, R_noise, K, Kff,
L)
gains.A_c = A_c
gains.B_c = B_c
gains.Q_c = Q_noise
return gains