예제 #1
0
	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
예제 #2
0
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)