Skip to content

Instantly share code, notes, and snippets.

@tsu-nera tsu-nera/pid_control.py
Last active Jul 23, 2017

Embed
What would you like to do?
P, PD, PID制御
import random
import numpy as np
import matplotlib.pyplot as plt
# ------------------------------------------------
#
# this is the Robot class
#
class Robot(object):
def __init__(self, length=20.0):
"""
Creates robot and initializes location/orientation to 0, 0, 0.
"""
self.x = 0.0
self.y = 0.0
self.orientation = 0.0
self.length = length
self.steering_noise = 0.0
self.distance_noise = 0.0
self.steering_drift = 0.0
def set(self, x, y, orientation):
"""
Sets a robot coordinate.
"""
self.x = x
self.y = y
self.orientation = orientation % (2.0 * np.pi)
def set_noise(self, steering_noise, distance_noise):
"""
Sets the noise parameters.
"""
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = steering_noise
self.distance_noise = distance_noise
def set_steering_drift(self, drift):
"""
Sets the systematical steering drift parameter
"""
self.steering_drift = drift
def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):
"""
steering = front wheel steering angle, limited by max_steering_angle
distance = total distance driven, most be non-negative
"""
if steering > max_steering_angle:
steering = max_steering_angle
if steering < -max_steering_angle:
steering = -max_steering_angle
if distance < 0.0:
distance = 0.0
# make a new copy
# res = Robot()
# res.length = self.length
# res.steering_noise = self.steering_noise
# res.distance_noise = self.distance_noise
# res.steering_drift = self.steering_drift
# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# apply steering drift
steering2 += self.steering_drift
# Execute motion
turn = np.tan(steering2) * distance2 / self.length
if abs(turn) < tolerance:
# approximate by straight line motion
self.x += distance2 * np.cos(self.orientation)
self.y += distance2 * np.sin(self.orientation)
self.orientation = (self.orientation + turn) % (2.0 * np.pi)
else:
# approximate bicycle model for motion
radius = distance2 / turn
cx = self.x - (np.sin(self.orientation) * radius)
cy = self.y + (np.cos(self.orientation) * radius)
self.orientation = (self.orientation + turn) % (2.0 * np.pi)
self.x = cx + (np.sin(self.orientation) * radius)
self.y = cy - (np.cos(self.orientation) * radius)
def __repr__(self):
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
############## ADD / MODIFY CODE BELOW ####################
# ------------------------------------------------------------------------
#
# run - does a single control run
# previous P controller
def run_p(robot, tau, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []
for i in range(n):
cte = robot.y
steer = -tau * cte
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)
return x_trajectory, y_trajectory
def run_pd(robot, tau_p, tau_d, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []
cte = robot.y
for i in range(n):
cte_diff = robot.y - cte
cte = robot.y
steer = -tau_p*cte - tau_d*cte_diff
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)
return x_trajectory, y_trajectory
def run_pid(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []
cte = robot.y
cte_total = 0
# TODO: your code here
for i in range(n):
cte_diff = robot.y - cte
cte = robot.y
cte_total += cte
steer = -tau_p*cte - tau_d*cte_diff - tau_i*cte_total
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)
return x_trajectory, y_trajectory
robot = Robot()
robot.set(0, 1, 0)
x_trajectory, y_trajectory = run_p(robot, 0.20)
n = len(x_trajectory)
plt.plot(x_trajectory, y_trajectory, 'g', label='P controller')
plt.plot(x_trajectory, np.zeros(n), 'r', label='reference')
plt.legend()
plt.show()
robot = Robot()
robot.set(0, 1, 0)
x_trajectory, y_trajectory = run_pd(robot, 0.2, 3.0)
n = len(x_trajectory)
plt.plot(x_trajectory, y_trajectory, 'g', label='PD controller')
plt.plot(x_trajectory, np.zeros(n), 'r', label='reference')
plt.legend()
plt.show()
robot = Robot()
robot.set(0, 1, 0)
x_trajectory, y_trajectory = run_pid(robot, 0.2, 3.0, 0.004)
n = len(x_trajectory)
plt.plot(x_trajectory, y_trajectory, 'g', label='PID controller')
plt.plot(x_trajectory, np.zeros(n), 'r', label='reference')
plt.legend()
plt.show()
@tsu-nera

This comment has been minimized.

Copy link
Owner Author

tsu-nera commented Jul 23, 2017

image
image
image

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.