def integrator(T):
"""
Takes as its input the motor’s angular velocity Ωh, and outputs the motor’s
angular position Θh
Args:
T (int): a timestep, the length of time between samples
Returns:
an instance of the SystemFunction class which represents an integrator
with the appropriate timestep
"""
system1 = sf.Cascade(sf.R(), sf.Gain(T))
system2 = sf.FeedbackAdd(sf.Gain(1), sf.R())
return sf.Cascade(system1, system2)
def motorModel(T):
"""
Takes V_c as input and generates Ωh as output through the equation
sf.Gain((k_m * T) / (k_b * k_m * T + r_m))
Args:
T (int): a timestep, the length of time between samples
Returns:
an instance of the SystemFunction class which represents a motor
"""
system1 = sf.Cascade(sf.Gain(k_m / r_m), sf.R())
system2 = sf.Cascade(sf.Gain(T), sf.FeedbackAdd(sf.Gain(1), sf.R()))
system3 = sf.Cascade(system1, system2)
return sf.FeedbackSubtract(system3, sf.Gain(k_b))
def angleModel(Kp, Ka):
T = 0.1
V = 0.1
plant1 = sf.SystemFunctional(poly.Polynomial([T, 0]),
poly.Polynomial([-1, 1]))
smallFeedbackNum = sf.Cascade(plant1, sf.Gain(Ka))
smallFeedbackDen = sf.Gain(1)
smallFeedback = sf.FeedbackSubtract(smallFeedbackNum, smallFeedbackDen)
firstCascade = sf.Cascade(sf.Gain(float(Kp) / Ka), smallFeedback)
plant2 = sf.SystemFunctional(poly.Polynomial([T * V, 0]),
poly.Polynomial([-1, 1]))
secondCascade = sf.Cascade(firstCascade, plant2)
return sf.FeedbackSubtract(secondCascade, sf.Gain(1))
def delayPlusPropModel(k1, k2):
T = 0.1
V = 0.1
# Controller: your code here
## controller = sf.Sum(sf.Gain(k1), sf.Cascade(sf.Gain(k2), sf.R()))
controller = sf.SystemFunction(poly.Polynomial([k2, k1]),
poly.Polynomial([1]))
# The plant is like the one for the proportional controller. Use
# your definition from last week.
plant1 = sf.Cascade(sf.Gain(0.1), sf.FeedbackAdd(sf.R(), sf.Gain(1)))
plant2 = sf.Cascade(sf.Gain(0.1 * 0.1), sf.FeedbackAdd(sf.R(), sf.Gain(1)))
# Combine the three parts
sys = sf.FeedbackSubtract(
sf.Cascade(controller, sf.Cascade(plant1, plant2)), sf.Gain(1))
return sys
def delayPlusPropModel(k1, k2):
"""
Args:
k1, k2 (int): gains
Returns:
An instance of the SystemFunction class that describes the behaviour of
the system when the robot has a delay plus-proportional controller
"""
T = 0.1
V = 0.1
# Controller
module1 = sf.Cascade(sd.R(), sf.Gain(k2))
controller = sf.FeedforwardAdd(sf.Gain(k1), module1)
# Plant 1
module1 = sf.Cascade(sf.R(), sf.Gain(T))
module2 = sf.FeedbackAdd(sf.Gain(1), sf.R())
plant1 = sf.Cascade(module1, module2)
# Plant 2
module1 = sf.Cascade(sf.Gain(V * T), sf.R())
module2 = sf.FeedbackAdd(sf.Gain(1), sf.R())
plant2 = sf.Cascade(module1, module2)
# Combine the three parts
module1 = sf.Cascade(controller, sf.Cascade(plant1, plant2))
sys = sf.FeedbackSubtract(module1)
return sys
def plantModel(T):
"""
Args:
T (int): a timestep, the length of time between samples
Returns:
an instance of the SystemFunction class which represents the entire
plant
"""
return sf.Cascade(motorModel(T), integrator(T))
def controllerAndSensorModel(k_c):
"""
Args:
k_c (float): a gain
Returns:
an instance of the SystemFunction class which represents the
controller/sensor system with the input gain
"""
return sf.Cascade(sf.Gain(k_s), sf.Gain(k_c))
def delayPlusPropModel(k1, k2): T = 0.1 V = 0.1 # Controller: your code here controller = sf.FeedbackSubtract(sf.Gain(k1), sf.Cascade(sf.Gain(k2), sf.R())) # The plant is like the one for the proportional controller. Use # your definition from last week.] plant1 = sf.Cascade(sf.Cascade(sf.Gain(T), sf.R()), sf.FeedbackAdd(sf.Gain(1), sf.R())) plant2 = sf.Cascade(sf.Cascade(sf.Gain(T * V), sf.R()), sf.FeedbackAdd(sf.Gain(1), sf.R())) # Combine the three parts sys = sf.FeedbackSubtract(sf.Cascade(sf.Cascade(controller, plant1), plant2)) return sys
def lightTrackerModel(T, k_c):
"""
Connects all of these systems together into one large system whose input Θ_l
is the angular position of the light, and whose output Θh is the angular
position of the head.
Args:
T (int): a timestep, the length of time between samples
k_c (float): the gain to use for the controller
Returns:
an instance of the SystemFunction class which represents the entire
light-tracking system with the specified gains and timesteps
"""
system = sf.Cascade(controllerAndSensorModel(k_c), plantModel(T))
return sf.FeedbackSubtract(system)
def anglePlusPropModel(k3, k4): T = 0.1 V = 0.1 # plant 1 is as before plant1 = sf.Cascade(sf.Cascade(sf.Gain(T), sf.R()), sf.FeedbackAdd(sf.Gain(1), sf.R())) # plant2 is as before plant2 = sf.Cascade(sf.Cascade(sf.Gain(T * V), sf.R()), sf.FeedbackAdd(sf.Gain(1), sf.R())) # The complete system sys = sf.FeedbackSubtract(sf.Cascade(sf.Cascade(sf.Gain(k3), sf.FeedbackSubtract(plant1, sf.Gain(k4))), plant2)) return sys
def delayPlusPropModel(k1, k2):
T = 0.1
V = 0.1
print k2
# Controller: your code here
# omega[n] = k1 * e[n] + k2 * e[n-1].
controller = sf.FeedforwardAdd(sf.Gain(k1),
sf.Cascade(sf.R(), sf.Gain(k2)))
# print controller
# The plant is like the one for the proportional controller. Use
# your definition from last week.
# θ[n]=T∙ω[n-1]+θ[n-1]
plant1 = sf.Cascade(sf.Cascade(sf.R(), sf.Gain(T)),
sf.FeedbackAdd(sf.Gain(1), sf.R()))
# d_0 [n]=VT∙sinθ[n-1]+d_0 [n-1]≈VT∙θ[n-1]+d_0 [n-1]
plant2 = sf.Cascade(sf.Cascade(sf.R(), sf.Gain(T * V)),
sf.FeedbackAdd(sf.Gain(1), sf.R()))
# Combine the three parts
sys = sf.FeedbackSubtract(
sf.Cascade(sf.Cascade(controller, plant1), plant2), sf.Gain(1))
# print sys
return sys
def anglePlusPropModel(k3, k4):
"""
Takes gains k3 and k4 as input and returns a SystemFunction that describes
the system with angle-plus proportional control
"""
T = 0.1
V = 0.1
# Plant 1 as before
module1 = sf.Cascade(sf.R(), sf.Gain(T))
module2 = sf.FeedbackAdd(sf.Gain(1), sf.R())
plant1 = sf.Cascade(module1, module2)
# Plant 2 as before
module1 = sf.Cascade(sf.Gain(V * T), sf.R())
module2 = sf.FeedbackAdd(sf.Gain(1), sf.R())
plant2 = sf.Cascade(module1, module2)
# The complete system
module1 = sf.FeedbackSubtract(plant1, sf.Gain(k4))
module2 = sf.Cascade(sf.Gain(k3), module1)
sys = sf.FeedbackSubtract(sf.Cascade(module2, plant2))
return sys
def wallFinderSystemSF(T, k):
return sf.Cascade(controllerSF(k), plantSF(T))
def plantSF(T):
return sf.Cascade(sf.Gain(-T), sf.FeedbackAdd(sf.R, sf.Gain(1)))
def wallFinderSystemSF(T, k):
return sf.FeedbackSubtract(sf.Cascade(controllerSF(k), plant(T)), sensorSF())
def wallFollowerModel(k, T, V):
one = sf.Cascade(sf.Cascade(controller(k), plant1(T)), plant2(T, V))
return sf.FeedbackSubtract(one, sf.Gain(1))
def plant2(T, V): return sf.Cascade(sf.Gain(V*T), sf.FeedbackAdd(sf.R(), sf.Gain(1)))
def plant1(T): return sf.Cascade(sf.Gain(T), sf.FeedbackAdd(sf.R(), sf.Gain(1)))