本文整理汇总了Python中pid.PID.calc方法的典型用法代码示例。如果您正苦于以下问题：Python PID.calc方法的具体用法？Python PID.calc怎么用？Python PID.calc使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类pid.PID的用法示例。
在下文中一共展示了PID.calc方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: control_velocity
# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import calc [as 别名]
class control_velocity(object):
def __init__(self):
self.linear_pid = PID(95, 85, 1, 62.915)
self.angular_pid = PID(6, 4, 0.1, 3.125)
self.linear_pid.setpoint = 0
self.angular_pid.setpoint = 0
self.left_pub = rospy.Publisher('/wheel_left/duty', Int16, queue_size=1)
self.right_pub = rospy.Publisher('/wheel_right/duty', Int16, queue_size=1)
rospy.Subscriber("/cmd_vel", Twist, self.update_setpoints)
rospy.Subscriber("/wheel_odom", Odometry, self.update_duty)
def update_setpoints(self, cmd_vel):
self.linear_pid.setpoint = cmd_vel.linear.x
self.angular_pid.setpoint = cmd_vel.angular.z
def update_duty(self, odom):
twist = odom.twist.twist
if self.linear_pid.setpoint == 0 and self.angular_pid.setpoint == 0:
self.linear_pid.clear_integrator()
self.angular_pid.clear_integrator()
linear = self.linear_pid.calc(twist.linear.x)
angular = self.angular_pid.calc(twist.angular.z)
self.left_pub.publish(Int16(-linear + angular))
self.right_pub.publish(Int16(-linear - angular))示例2: Controller
# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import calc [as 别名]
class Controller(object):
def __init__(self):
self.left_wall_pid = PID(14, 2, 0.5)
self.right_wall_pid = PID(14, 2, 0.5)
self.left_wall_pid.setpoint = 0.055
self.right_wall_pid.setpoint = 0.055
self.goal_angle = None
self.goal_distance = None
self.start_position = None
self.position = None
self.start_angle = None
self.angle = None
self.left_dist = 0
self.right_dist = 0
self.front_dist = 0
self.commands = []
self.busy = False
self.callback = None
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rospy.Subscriber("/wheel_odom", Odometry, self.update)
rospy.Subscriber("/sensors/ir_left", Float32, self.ir_left)
rospy.Subscriber("/sensors/ir_right", Float32, self.ir_right)
rospy.Subscriber("/sensors/ir_front", Float32, self.ir_front)
def ir_left(self, data):
print("ASJKFLAJKFSLJAKSHELLOOOOOOOOOOOOOOOOOO")
self.left_dist = data.data
def ir_right(self, data):
self.right_dist = data.data
def ir_front(self, data):
self.front_dist = data.data
def update(self, odom):
# populate current state variables
pose = odom.pose.pose
twist = odom.twist.twist
q = pose.orientation
self.angle = transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
self.position = pose.position
if not self.busy and self.commands:
data = self.commands.pop(0)
data[0]()
self.callback = data[1]
print("Starting action: " + data[2])
self.busy = True
# traverse if needed
cmd_vel = Twist()
linear_vel = 0
angular_vel = 0
if self.goal_distance != None:
# heading correction
if self.left_dist > 0.03 and self.left_dist < 0.08:
angular_vel -= self.left_wall_pid.calc(self.left_dist)
if self.right_dist > 0.03 and self.right_dist < 0.08:
angular_vel += self.right_wall_pid.calc(self.right_dist)
# vroom
offset = abs(self.goal_distance) - dist(self.position.x, self.position.y, self.start_position.x, self.start_position.y)
if abs(offset) < 0.01 and abs(twist.linear.x) < 0.01:
self.goal_distance = None
elif abs(offset) < 0.05:
linear_vel = signum(offset) * 0.12
else:
linear_vel = signum(offset) * 0.18
# absolute position correction via front IR
if offset > 0.08 and offset < 0.18:
error = self.front_dist - offset - 0.015
if abs(error) < 0.06:
self.goal_distance += error
if self.goal_angle != None:
offset = wrap_angle(self.goal_angle - self.angle)
# print(offset)
if abs(offset) < 0.06 and abs(twist.angular.z) < 0.2 and signum(offset) == -signum(twist.angular.z):
self.goal_angle = None
elif abs(offset) < 0.05:
angular_vel = signum(offset) * 1.5
elif abs(offset) < 0.2:
angular_vel = signum(offset) * 2.5
else:
angular_vel = signum(offset) * 5
if self.busy and self.goal_angle == None and self.goal_distance == None:
self.busy = False
print('DONE')
linear_vel = 0
#.........这里部分代码省略.........