Python PID.start方法代码示例

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import start [as 别名]
class MotionControl(object):
    def __init__(self):
        rospy.on_shutdown(self.stop)
        rospy.wait_for_service('uniboard_service')
        self.uniboard_service = rospy.ServiceProxy('uniboard_service', communication)
        rospy.Subscriber("/cmd_vel", Twist, self.set_vel)
        rospy.Subscriber("/vel", vel_update, self.update)
        self.vel_pid = PID(self.drive, 0.5, 0.01, 0.0,[-2, 2], [-1, 1], 'vel_pid')
        self.vel_pid.start()
        self.rot_pid = PID(self.rotation, 0.5, 0.01, 0.0, [-2.0, 2.0], [-0.5, 0.5], 'rot_pid')
        self.rot_pid.start()
        # The offset value between wheel power to drive rotation
        self.rotation_offset = 0.0

    def drive(self, power):
        self.uniboard_service('motor_left', 3, str(power), rospy.Time.now())
        self.uniboard_service('motor_right', 3, str(power+self.rotation_offset), rospy.Time.now())

    def set_vel(self, twist):
        self.vel_pid.set_target(twist.linear.x)
        self.rot_pid.set_target(twist.angular.z)


    def rotation(self, diff):
        self.rotation_offset = diff

    def stop(self):
        self.vel_pid.stop()
        self.rot_pid.stop()

    def start(self):
        self.vel_pid.start()
        self.vel_pid.reset()
        self.rot_pid.start()
        self.rot_pid.reset()

    def update(self, vel):
        linear = vel.linear_x
        angular = vel.angular_z
        self.vel_pid.update(linear)
        self.rot_pid.update(angular)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import start [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 start [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)