Python PID.onTarget方法代码示例

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import onTarget [as 别名]
class GyroAndCompassRobot(TwoWheeledRobot):
	def __init__(self):
		super(GyroAndCompassRobot, self).__init__()

		self.gyro = Gyro()
		self.compass = Compass()

		self.compassRegulator = PID(
			getInput = lambda: self.compass.heading,
			setOutput = lambda x: TwoWheeledRobot.drive(self, speed = 0, steer = x),
			outputRange = (-100, 100),
			iLimit = 0.25
		)
		self.gyroRegulator = PID(
			getInput = lambda: self.gyro.angularVelocity,
			setOutput = lambda x: TwoWheeledRobot.drive(self, speed = self.speed, steer = x),
			outputRange = (-100, 100),
			iLimit = 0.25
		)
		#self.regulator.tuneFromZieglerNichols(2.575, 0.698)

		#Compass PID settings
		self.compassRegulator.kp =  2.75 # FIRST this number started at 0 and was raised until it started to oscillate
		self.compassRegulator.ki =  1.5  # THIRD we changed until it stopped dead on.
		self.compassRegulator.kd =  0.2  # SECOND we changed kd until the amount it overshot by was reduced

		#Gyro PID settings
		self.gyroRegulator.kp = 3   # FIRST this number started at 0 and was raised until it started to oscillate
		self.gyroRegulator.ki = 7.5 # THIRD we changed until it stopped dead on.
		self.gyroRegulator.kd = 0   # SECOND we changed kd until the amount it overshot by was reduced
		self.gyroRegulator.target = 0


		self.compassRegulator.start()
		self.gyroRegulator.start()

		self.speed = 0

	@logs.to(logs.movement)
	def drive(self, speed, steer = 0, regulate = True):
		"""
		Drive the robot at a certain speed, by default using the compass to
		regulate the robot's heading.

		TODO: Tune PID controller for straight movement.
		"""
		if regulate:
			self.compassRegulator.enabled = False

			self.speed = speed
			self.gyro.calibrate()
			self.gyroRegulator.target = steer
			self.gyroRegulator.enabled = True
		else:
			TwoWheeledRobot.drive(self, speed, steer)

	@logs.to(logs.movement)	
	def rotateTo(self, angle, tolerance = 5):
		self.gyroRegulator.enabled = False
		self.compassRegulator.target = angle
		self.speed = 0

		self.compassRegulator.enabled = True
		while not self.compassRegulator.onTarget(tolerance=tolerance):
			time.sleep(0.05)

	def rotateBy(self, angle, fromTarget = False, tolerance = 5):
		"""
		Rotate the robot a certain angle from the direction it is currently
		facing. Optionally rotate from the last target, preventing errors
		accumulating
		"""
		self.rotateTo((self.compassRegulator.target if fromTarget else self.compass.heading) + angle, tolerance = tolerance)
		
	def stop(self):
		"""
		Stop the robot, by setting the speed to 0, and disabling regulation
		"""
		self.speed = 0
		self.gyroRegulator.enabled = False
		self.compassRegulator.enabled = False
		super(GyroAndCompassRobot, self).stop()

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import onTarget [as 别名]
class CompassRobot(TwoWheeledRobot):
	def __init__(self):
		TwoWheeledRobot.__init__(self)
		self.compass = Compass()

		self.regulator = PID(
			getInput = lambda: self.compass.heading,
			setOutput = lambda x: TwoWheeledRobot.drive(self, speed = self.speed, steer = x),
			outputRange = (-100, 100),
			iLimit = 0.25
		)
		#self.regulator.tuneFromZieglerNichols(2.575, 0.698)

		#PID settings
		self.regulator.kp = 2.75 #3.2 #1.500 # FIRST this number started at 0 and was raised until it started to oscillate
		self.regulator.ki = 1.5 #0.175 # THIRD we changed until it stopped dead on.
		self.regulator.kd = 0.2 #0.194 #0.080 # SECOND we changed kd until the amount it overshot by was reduced

		self.regulator.start()

		self.speed = 0
		
	@property
	def regulate(self):
		"""Shorthand for enabling and disabling the PID controller"""
		return self.regulator.enabled
		
	@regulate.setter
	def regulate(self, value):
		self.regulator.enabled = value
		 
	def rotateTo(self, heading, tolerance = 2.5):
		"""
		Rotate the robot to face the specified heading. Return when within
		tolerance degrees of the target. Note that this does not stop the motors
		upon returning
		"""
		self.regulate = True;
		self.speed = 0
		self.regulator.target = heading
		
		while not self.regulator.onTarget(tolerance=tolerance):
			time.sleep(0.05)
		
	def rotateBy(self, angle, fromTarget = False):
		"""
		Rotate the robot a certain angle from the direction it is currently
		facing. Optionally rotate from the last target, preventing errors
		accumulating
		"""
		self.rotateTo((self.regulator.target if fromTarget else self.compass.heading) + angle)
		
	def setSpeed(self, speed):
		print "deprecated - use drive instead"
		self.drive(speed, regulate = True)

	def drive(self, speed, steer = 0, regulate = True):
		"""
		Drive the robot at a certain speed, by default using the compass to
		regulate the robot's heading.

		TODO: Tune PID controller for straight movement.
		"""
		self.regulate = regulate
		if regulate:
			self.speed = speed
		else:
			TwoWheeledRobot.drive(self, speed, steer)
			
		
	def stop(self):
		"""
		Stop the robot, by setting the speed to 0, and disabling regulation
		"""
		self.speed = 0
		self.regulate = False
		TwoWheeledRobot.stop(self)