def wallFollowerModel(K, T=0.1, V=0.1):
multipliedResult = K * (T**2) * V
num1 = poly.Polynomial([multipliedResult, 0, 0])
den1 = poly.Polynomial([1, -2, 1])
top = sf.SystemFunctional(num1, den1)
return sf.FeedbackSubtract(
top, sf.SystemFunctional(poly.Polynomial([1]), poly.Polynomial([1])))
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 makeSF(K):
num = poly.Polynomial([K, 0])
den = poly.Polynomial([K, -1, 1])
return sf.SystemFunctional(num, den)
def makeSF(K):
return sf.SystemFunctional(poly.Polynomial([K,0]),poly.Polynomial([K,-1,1]))
def wallFollowerModel(K, T=0.1, V=0.1):
numerator = poly.polynomial([K * T * T * V, 0, 0])
denominator = poly.polynomial([K * T * T * V + 1, -2, 1])
result = sf.SystemFunctional(numerator, denominator)
return result
import math
import lib601.poly as poly
import lib601.sf as sf
def wallFollowerModel(K, T=0.1, V=0.1):
numerator = poly.polynomial([K * T * T * V, 0, 0])
denominator = poly.polynomial([K * T * T * V + 1, -2, 1])
result = sf.SystemFunctional(numerator, denominator)
return result
def periodOfPole(pole):
real = pole.real
imag = pole.imag
if math.atan2(imag, real) == 0:
return real
else:
return abs(2 * math.pi / math.atan2(imag, real))
def Pole(K):
return (2 + math.sqrt(abs(4 - 4 * (1 + K * 0.1 * 0.1 * 0.1)))) / 2
for k in [0.2, 1, 10, 50, 100]:
numerator = poly.Polynomial([k * 0.1 * 0.1 * 0.1, 0, 0])
denominator = poly.Polynomial([k * 0.1 * 0.1 * 0.1 + 1, -2, 1])
result = sf.SystemFunctional(numerator, denominator)
print result.dominantPole(), periodOfPole(result.dominantPole())