def __init__(self, dt, name, num_stages_per_side, num_motors_per_side, stall_current, free_current, stall_torque,
free_speed, gear_ratio_per_side, moment_of_inertia, wheel_radius, drivebase_radius, mass,
C = numpy.matrix([[1 , 0, 0, 0], [0,1,0,0]])):
"""Instantiates the drive template. Units should be SI. Free speed
should be radians per second, gear ratio should be greater than one,
stall current, free current, stall torque just refer to your motor specs for one
of that type which should be found on Vex Motors. The dt refers to the robot's
loop speed which as of 2018, is 5 ms (so dt = 0.005), and name refers to what
you want to call the subsystem. Drivebase radius is a fudge factor for the
effects of the drivetrain on each side, a reasonable approximation is half
the length of the drivetrain base."""
state_space_controller.__init__(self, dt, name)
#Input values
self.N = num_stages_per_side
self.n = num_motors_per_side
self.I_stall = stall_current * self.n
self.I_free = free_current * self.n
self.tau_stall = stall_torque * self.n
self.omega_free = free_speed
self.J = moment_of_inertia
self.G = gear_ratio_per_side
self.r_w = wheel_radius
self.r_b = drivebase_radius
self.m = mass
#Derived Values
#efficiency
self.mu = 0.95 ** self.N
#resistance
self.R = 12.0 / self.I_stall
self.k_t = self.I_stall / self.tau_stall
self.k_v = (12.0 - self.I_free * self.R) / self.omega_free
self.gamma_1 = (1.0 / self.m + self.r_b * self.r_b / self.J)#just placeholder constant so I don't retype all the variables
self.gamma_2 = (1.0 / self.m - self.r_b * self.r_b / self.J)
#coefficient of velocity
self.k_vel = -self.k_v * self.G * self.G / (self.k_t * self.R * self.r_w * self.r_w)
#coefficient of input
self.k_u = self.mu * self.G / (self.k_t * self.R * self.r_w)
#State is [[pos_left],[pos_right],[vel_left],[vel_right]]
self.A_c = numpy.matrix([[0, 0, 1.0, 0],
[0, 0, 0, 1.0],
[0, 0, self.gamma_1 * self.k_vel, self.gamma_2 * self.k_vel],
[0 ,0, self.gamma_2 * self.k_vel, self.gamma_1 * self.k_vel]])
self.B_c = numpy.matrix([[0, 0],
[0, 0],
[self.gamma_1 * self.k_u, self.gamma_2 * self.k_u],
[self.gamma_2 * self.k_u, self.gamma_1 * self.k_u]])
self.C = C
self.D = numpy.matrix([[0, 0],
[0, 0]])
#converts continuous to discrete matrices
self.A_d, self.B_d = controls.c2d(self.A_c,self.B_c,self.dt)
self.minU = numpy.matrix([[-12.0], [-12.0]])
self.maxU = numpy.matrix([[12.0], [12.0]])
self.initialize_state()
def __init__(self,
dt,
name,
num_stages,
num_motors,
stall_current,
free_current,
stall_torque,
free_speed,
moment_of_inertia,
gear_ratio,
C=numpy.matrix([[1, 0]])):
state_space_controller.__init__(self, dt, name)
"""Instantiates the arm template. Units should be SI. Free speed
should be radians per second, gear ratio should be greater than one,
stall current, free current, stall torque just refer to your motor specs for one
of that type which should be found on Vex Motors. The dt refers to the robot's
loop speed which as of 2018, is 5 ms (so dt = 0.005), and name refers to what
you want to call the subsystem."""
#Input values
self.N = num_stages
self.n = num_motors
self.I_stall = stall_current * num_motors
self.I_free = free_current * num_motors
self.tau_stall = stall_torque * num_motors
self.omega_free = free_speed
self.J = moment_of_inertia
self.G = gear_ratio
#Derived Values
#efficiency
self.mu = 0.95**self.N
#resistance
self.R = 12.0 / self.I_stall
self.k_t = self.I_stall / self.tau_stall
self.k_v = (12.0 - self.I_free * self.R) / self.omega_free
#coefficient of velocity
self.k_omega = -self.k_v * self.G * self.G / (self.k_t * self.R *
self.J)
#coefficient of input
self.k_u = self.mu * self.G / (self.k_t * self.R * self.J)
#State is [[angle],[angular_velocity]]
self.A_c = numpy.matrix([[0, 1], [0, self.k_omega]])
self.B_c = numpy.matrix([[0], [self.k_u]])
self.C = C
self.D = numpy.matrix([[0]])
#converts continuous to discrete matrices
self.A_d, self.B_d = controls.c2d(self.A_c, self.B_c, self.dt)
self.minU = numpy.matrix([[-12.0]])
self.maxU = numpy.matrix([[12.0]])
self.time_elapsed = 0.0
self.initialize_state()