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())