def __init__(self, dt): """Initialize the system dt : sampling period""" self.ml = FirstOrder(dt, simu_config.Tleft * (1 + (random() - 0.5)*2*simu_config.alea_Tleft), simu_config.Kleft * (1 + (random() - 0.5)*2*simu_config.alea_Kleft)) self.mr = FirstOrder(dt, simu_config.Tright * (1 + (random() - 0.5)*2*simu_config.alea_Tright), simu_config.Kright * (1 + (random() - 0.5)*2*simu_config.alea_Kright)) self.theta = Integral(dt) self.curv = Integral(dt) self.x = Integral(dt) self.y = Integral(dt) self.dt = dt self.ticspermeter = simu_config.ticspermeter self.diffticsperrad = simu_config.diffticsperrad
class System: def __init__(self, dt): """Initialize the system dt : sampling period""" self.ml = FirstOrder(dt, simu_config.Tleft * (1 + (random() - 0.5)*2*simu_config.alea_Tleft), simu_config.Kleft * (1 + (random() - 0.5)*2*simu_config.alea_Kleft)) self.mr = FirstOrder(dt, simu_config.Tright * (1 + (random() - 0.5)*2*simu_config.alea_Tright), simu_config.Kright * (1 + (random() - 0.5)*2*simu_config.alea_Kright)) self.theta = Integral(dt) self.curv = Integral(dt) self.x = Integral(dt) self.y = Integral(dt) self.dt = dt self.ticspermeter = simu_config.ticspermeter self.diffticsperrad = simu_config.diffticsperrad def process(self, cl, cr): """Compute the system response cl : command to apply to left motor cr : command to apply to right motor""" # Saturation if cl < -1: cl = -1 if cr < -1: cr = -1 if cl > 1: cl = 1 if cr > 1: cr = 1 # Motor process vl = self.ml.process(cl) # tics / s vr = self.mr.process(cr) # tics / s # Polar velocity vmean = (vl + vr) / 2 / self.ticspermeter # m/s vang = (vl - vr) / self.diffticsperrad # rad/s # Angular and curvilign integration curv = self.curv.process(vmean) # m theta = self.theta.process(vang) # rad # XY position self.x.process(vmean * cos(theta)) # m self.y.process(vmean * sin(theta)) # m return (x, y)
