def test_pid(P=0.2, I=0.0, D=0.0, L=100):
"""Self-test PID class
.. note::
...
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1/i))
if i>9:
pid.SetPoint = 1
time.sleep(0.02)
---
"""
pid = PID(P, I, D)
pid.SetPoint = 0.0
pid.setSampleTime(0.01)
END = L
feedback = 0
feedback_list = []
time_list = []
setpoint_list = []
for i in range(1, END):
pid.update(feedback)
output = pid.output
print output
if pid.SetPoint > 0:
feedback += output # (output - (1/i))控制系统的函数
if i > 9:
pid.SetPoint = 10
time.sleep(0.01)
feedback_list.append(feedback)
setpoint_list.append(pid.SetPoint)
time_list.append(i)
time_sm = np.array(time_list)
time_smooth = np.linspace(time_sm.min(), time_sm.max(), 300)
feedback_smooth = spline(time_list, feedback_list, time_smooth)
plt.figure(0)
plt.plot(time_smooth, feedback_smooth)
plt.plot(time_list, setpoint_list)
plt.xlim((0, L))
plt.ylim((min(feedback_list) - 0.5, max(feedback_list) + 0.5))
plt.xlabel('time (s)')
plt.ylabel('PID (PV)')
plt.title('TEST PID')
plt.ylim((1 - 10.5, 1 + 10.5))
# plt.ylim((1-0.5, 1+0.5))
plt.grid(True)
plt.show()
except Exception as e:
#print str(e)
continue
#rospy.sleep(1) #Wait a second for a first goal to come in.
#continue
#Do the actual control here!
x = localGoal.pose.position.x
y = localGoal.pose.position.y
theta_err = atan2(
-y,
x) #different coordinate systems. Here x=1,y=0 is straight ahead.
#phi = -1.0*theta_err
phi = angle_pid.update(theta_err, t_now.to_sec())
phi = max(min(phi, 1.5),
-1.5) #maximum hardware turning rate for turtlebot
print angle_pid.debug()
if hypot(x, y) > .05:
v = .3
else:
v = 0
phi = 0
#print("localx:",x,"localy:",y)
#(v,phi) = CalculateWheelVelocity(-y,x) #Positive X is forward in the robot's frame, -y is right
#rospy.spin()
move_cmd = Twist()
move_cmd.linear.x = v
move_cmd.angular.z = phi
pub.publish(move_cmd)