def main():
rod_weight = 4.0 * 0.293 * 0.453592
radius = 3.0 / 16.0 * 0.0254
pitch = 16.0/ (1 * 0.0254)
load = calculate_load_torque(40. * 4.4482, 2.0 * radius, pitch, 0.19)
test = linear_actuator(0.005, "test", 1.0, 1.0, 105.0, 1.8, 2.6, 5676 * 2.0 * math.pi / 60.0,
0.5 * rod_weight * radius * radius, 1.0, pitch, load)
#test.run_custom_test(numpy.matrix([[0.0],[0]]), numpy.matrix([[24.0],[0]]), max_func, True, False, True, 800)
#test.get_stats()
best_G = calculate_ideal_gear_ratio(load, 1.3)
print(best_G)
#Neo @ 1:3 overdrive running at 10 volts works best
drive_gear_ratio = 7.083 * 24.0 / 60.0 * 64.0 / 20.0;#1.13333#5.10#50.0 /34.0 2.16 ratio spread 54 : 30
j = 6.0
#2778 2000 2907 3269 3211 3099 3287 3573 2705
#drivetrain = drive(0.005, "drivetrain", 4.0, 2.0, 131.0, 2.7, 2.41, 5330 * 2.0 * math.pi / 60.0,
# drive_gear_ratio, j, 3.0 * 0.0254, 28.0 * 0.0254 * 0.5, 154.0 * 0.454)
#radius base might be 25.5 * 0.0254 * 0.5, but not likely, more like 0.45
drivetrain = drive(0.005, "drivetrain", 3.0, 2.0, 131.0, 2.7, 2.41, 5330 * 2.0 * math.pi / 60.0,
drive_gear_ratio, j, 3.0 * 0.0254, 25.5 * 0.0254 * 0.5 , (125.0) * 0.454)
#drivetrain.run_pid_test([1.0, 0.0,0],numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[55.0],[55.0], [0.0],[0.0]]), True, True, 4000)
#drivetrain.run_custom_test(numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[12.0],[12.0], [0.0],[0.0]]), full_volts, True, False, True, 2000)
drivetrain.get_stats(True)
driveQ_c = numpy.matrix([[1.0 / (0.14)**2.0 , 0, 0, 0],
[0, 1.0 / (0.14)**2.0, 0, 0],
[0, 0, 1.0 / (1.0)**2.0, 0],
[0, 0, 0, 1.0 / (1.0)**2.0]])
driveR_c = numpy.matrix([[0.0001**2, 0.0],
[0.0, 0.0001 **2]])
# [0 , 1.0 / (.1 * 0.0254)**2]])
driveA_d, driveB_d, driveQ_d, driveR_d = controls.c2d(drivetrain.A_c, drivetrain.B_c, 0.005, driveQ_c, driveR_c)
driveK = controls.dlqr(driveA_d, driveB_d, driveQ_d, driveR_d)
print(driveK)
elev = elevator(0.005, "elev", 2.0, 2.0, 134.0, 0.7, 0.71, 18730.0 * 2.0 * math.pi / 60.0,
63.0, 3.0 * (0.8755 - 0.16) * 0.0254, 18.8* 0.454)#11.59)
#elev.export("../python")
elevQ_c = numpy.matrix([[1.0 / (0.0254 * 0.01)**2.0 , 0],
[0 , 1.0 / (.1 * 0.0254)**2]])
elevR_c = numpy.matrix([[1.0 / 13.0**2]])
elevA_d, elevB_d, elevQ_d, elevR_d = controls.c2d(elev.A_c, elev.B_c, 0.005, elevQ_c, elevR_c)
elevK = controls.dlqr(elevA_d, elevB_d, elevQ_d, elevR_d)
elevL = controls.dkalman(elevA_d, elevB_d, elevQ_d, elevR_d)
#print(elevK)
elev.run_test([[0.],[0.]],[[0.0],[60.0]])
def kalman(plant):
L = controls.dkalman(plant.A_d, plant.C, plant.Q_d, plant.R_d)
return state_space_observer(plant, L, plant.x)
def __init__(self):
n = 2
tau_stall = 0.71 * n #N/m
I_stall = 134 * n# A
omega_free = 18730.0 * 2.0 * math.pi / 60.0 # rad / s
V_max = 12.0 #V
I_free = 0.7 * n
mu = 1.0
G = 63.0
#resistance
R = V_max / I_stall
#voltage constant
k_v = (V_max - I_free * R) / omega_free
#torque constant
k_t = I_stall / tau_stall
#pulley_radius
r_p = 0.0254
m = 10.0 * 0.454
A_11 = -(k_v * G * G) / (k_t * R * r_p * r_p * m)
B_01 = (G * mu) / (k_t * R * r_p * m)
A_c = np.asmatrix([[0.0, 1.0],
[0.0, A_11]])
B_c = np.asmatrix([[0.0],
[B_01]])
C = np.asmatrix([[1.0, 0.0]])
D = np.asmatrix([[0.0]])
Q_lqr = np.asmatrix([[3.0 ** 2, 0.0],
[0.0, 1.5 ** 2]])
Q_kal = np.asmatrix([[.01 ** 2, 0.0],
[0.0, .01 ** 2]])
R = np.asmatrix([[0.001 ** 2]])
self.minU = np.asmatrix([[-V_max]])
self.maxU = np.asmatrix([[V_max]])
A, B, Q_d, R_d = controls.c2d(A_c, B_c, 0.005, Q_lqr, R)
A, B, Q_kalman, R_d = controls.c2d(A_c, B_c, 0.005, Q_kal, R)
self.dt = 0.005
self.controller = ss.state_space_controller(A, B, C, D)
#pole placement
#self.K = controls.place(A, B, (0.7, 0.1))
#self.Kff = controls.feedforwards(A, B)
#self.controller.L = controls.place(A.T, C.T, (-0.61939275, 0.98497246)).T
#LQG
self.K = controls.dlqr(A, B, Q_d, R_d)
print(np.linalg.eig(A - B * self.K))
self.Kff = controls.feedforwards(A, B, Q_d)
self.controller.L = controls.dkalman(A, C, Q_kalman, R_d)
print(np.linalg.eig(A.T - C.T * self.controller.L.T))
def __init__(self, dt, name):
ss.StateSpaceController.__init__(self, dt)
self.origin = (0, 0)
self.time_elapsed = 0.0
self.has_cube = False
self.free_speed = 18730.0 * 2.0 * math.pi / 60.0 #radians per second
self.free_current = 0.7 #amps
self.stall_torque = 0.71 # newton meters
self.stall_current = 134.0
self.resistence = 12.0 / self.stall_current
self.k_t = self.stall_torque / self.stall_current
self.k_v = (12.0 -
self.free_current * self.resistence) / self.free_speed
self.gear_ratio = 833.33
self.mass = 10.0 * 0.454 #kilograms
if (self.has_cube):
self.mass += 1.59
self.radius = 17.0 * 0.0254 #meters
self.moment_of_inertia = self.mass * self.radius * self.radius #10.0 #this is speculative
self.efficiency = 0.65
self.a = (-self.k_t * self.k_v * self.gear_ratio *
self.gear_ratio) / (self.moment_of_inertia * self.resistence)
self.b = (self.efficiency * self.k_t *
self.gear_ratio) / (self.moment_of_inertia * self.resistence)
#state is [angle,
# angular_velocity]
#input is [voltage]
self.A_c = numpy.matrix([[0, 1], [0, self.a]])
self.B_c = numpy.matrix([[0], [self.b]])
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
q_1 = 1.0 / ((math.pi / 90.0)**2)
q_2 = 1.0 / ((math.pi / 90.0)**2)
self.Q_c = numpy.matrix([[q_1, 0], [0, q_2]])
r_1 = 1.0 / (0.1**2)
self.R_c = numpy.matrix([[r_1]])
self.temp = controls.clqr(self.A_c, self.B_c, self.Q_c, self.R_c)
print(controls.eig(self.A_c - self.B_c * self.temp))
self.A_d, self.B_d, self.Q_d, self.R_d = controls.c2d(
self.A_c, self.B_c, self.dt, self.Q_c, self.R_c)
#s = 0,-295.41341225 are open loop poles
#converting s to z is through e^(sT) where T is the discrete time step.
#(0.995 , 0.874) these are when s = -1, and s = -27
#tried with poles at 4.2, -295.41341225
poles = (cmath.exp(0.005 * (-27.0)), cmath.exp(0.005 * (-28.0)))
polesd = (-27.0 + 3.0j, -27.0 - 3.0j)
self.K = controls.place(self.A_d, self.B_d, poles)
self.Kff = controls.feedforwards(self.A_d, self.B_d, self.Q_d)
#These should be chosen 10 times faster in the s domain, and then converted
#to the z domain
#(0.951, 0.259) these are when s = -10, s= -270
self.L = controls.dkalman(self.A_d, self.C, self.Q_d, self.R_d)
self.minU = numpy.matrix([[-12.0]])
self.maxU = numpy.matrix([[12.0]])
self.initialize_state()
x_initial = np.asmatrix([0, 0]).T
# Controller weighting factors
Q_c = np.asmatrix([[10, 0], [0, 100]])
R_c = np.asmatrix([[1]])
# Noise properties
Q_o = np.asmatrix([[1, 0], [0, 1]])
R_o = np.asmatrix([[0.3]])
# Calculate gain matrices and discrete-time matrices
A_d, B_d, Q_d, R_d = controls.c2d(A, B, dt, Q_o, R_o)
K = controls.clqr(A, B, Q_c, R_c)
Kff = controls.feedforwards(A_d, B_d)
L = controls.dkalman(A_d, C, Q_d, R_d)
# Create a single set of gains
gains = StateSpaceGains('test_gains', dt, A_d, B_d, C, D, Q_d, R_o, K, Kff, L)
gains.add_writable_constant('A_c', A)
gains.add_writable_constant('B_c', B)
u_max = np.asmatrix([[12]])
plant = StateSpacePlant(gains, x_initial)
controller = StateSpaceController(gains, -u_max, u_max)
observer = StateSpaceObserver(gains, x_initial)
# Create and run the scenario
scenario = StateSpaceScenario(plant, x_initial, controller, observer,