def __init__(self, city_name):
self.timer = Timer()
# Map setup
self._map = CarlaMap(city_name)
self._centerlines = Centerlines(city_name)
# Agent Setup
Agent.__init__(self)
self._neural_net = CAL_network()
self._seq_len = 5 #self._neural_net.model.max_input_shape
self._state = VehicleState()
self._agents_present = False
# Controller setup
param_path = os.path.dirname(__file__) + '/controller/params/'
cruise_params = get_params_from_txt(param_path + 'cruise_params.txt')
self._PID_cruise = PID(*cruise_params)
follow_params = get_params_from_txt(param_path + 'follow_params.txt')
self._PID_follow = PID(*follow_params)
# General Parameter setup
general_params = get_params_from_txt(param_path + 'general_params.txt')
self.c, self.d = general_params[0], general_params[1]
self.Kl_STANLEY = general_params[2]
self.Kr_STANLEY = general_params[3]
self.K0_STANLEY = general_params[4]
self.curve_slowdown = general_params[5]
self.DELTAl = general_params[6]
self.DELTAr = general_params[7]
self.DELTA0 = general_params[8]
self.EXP_DECAY = general_params[9]
def __init__(self):
# Create a node that
# Subscribes to the laser scan topic,
# Publishes to drive topic - to move the vehicle.
# Initialize subscriber to laser scan.
rospy.Subscriber(self.SCAN_TOPIC, LaserScan, self.LaserCb)
# Initialize a publisher of drive commands.
self.drive_pub = rospy.Publisher(self.DRIVE_TOPIC,
Twist,
queue_size=10)
# Variables to keep track of drive commands being sent to robot.
self.seq_id = 0
# Class variables for following.
self.side_angle_window_fwd_ = math.pi * 0.1
self.side_angle_window_bwd_ = math.pi - math.pi * 0.3
self.point_buffer_x_ = np.array([])
self.point_buffer_y_ = np.array([])
self.num_readings_in_buffer_ = 0
self.num_readings_for_fit_ = 2
self.reject_dist = 0.7
self.steer_cmd = 0
self.vel_cmd = self.VELOCITY
self.pid = PID()
def apply_takeoff_controls():
# set wind environment
wind_speed, wind_heading = set_winds()
throttle_controller = PID(2.0, 0.0, 1.0, 10.0, sample_time)
rudder_controller = PID(0.3, 0.4, 1.5, 10.0, sample_time)
read_drefs = XPlaneDefs.control_dref + XPlaneDefs.position_dref
maximum_cle = 0
controls = None
start = time.time()
for t in range(int(simulation_steps // receding_horizon)):
gs, psi, throttle, x, _, z = xp_client.getDREFs(read_drefs)
gs, psi, throttle, x, z = gs[0], psi[0], throttle[0], x[0], z[0]
if TAKEOFF:
break
# cle = rejection_dist(desired_x, desired_z, x - origin_x, z - origin_z)
# maximum_cle = max(cld, maximum_cle)
dist = signed_rejection_dist(center_line, x - origin_x, z - origin_z)
dist = confine_bins(dist, c_bins)
psi = confine_bins(psi - runway_heading, h_bins)
gs = confine_bins(gs, v_bins)
wind_speed = confine_bins(wind_speed, wsp_bins)
wind_heading = confine_bins(wind_heading, whe_bins)
controls, cost = table[(dist, psi, gs, wind_speed, wind_heading)]
# change wind conditions every second
if time.time() - start > 1:
wind_speed, wind_heading = set_winds()
start = time.time()
# let PID controller take over
apply_lookup_controls(xp_client, throttle_controller,
rudder_controller, controls, sample_time,
receding_horizon)
return maximum_cle
def __init__(self):
super().__init__(0.25)
self.pid = PID(60, 30, 20, -50, 50)
self.pid.difference = compass.angleDifference
self.drive_forward = False
self.drive_backward = False
self.t = 0
self.last = 0
self.r_speed = 100
self.l_speed = 100
def test_pid(self):
pid_settings = PIDSettings()
pid_settings.B = 1
pid_settings.Ki = 5
pid_settings.Kd = 0.2
pid_settings.Kp = 19
pid_settings.Tt = 1 / pid_settings.Ki
pid_settings.Ts = 1
pid_settings.YMin = -10
pid_settings.YMax = 10
self.do_test(PID(), pid_settings, PIDInput, PIDParameters)
def turn_to(heading, error=math.radians(4)):
pid = PID(3, 0.1, 5, -50, 50)
pid.set_target(heading)
pid.difference = compass.angleDifference
h = compass.getHeading()
while True:
ret = pid.update(h)
if ret < 0:
motors.right(50 - ret)
elif ret > 0:
motors.left(50 + ret)
if abs(compass.angleDifference(h, heading)) <= error:
motors.stop()
time.sleep(0.5)
if abs(compass.angleDifference(compass.getHeading(),
heading)) <= error:
break
h = compass.getHeading()
time.sleep(0.05)
model_name = sys.argv[1]
target_name = sys.argv[2]
target_func = load_target_func(target_name)
elif len(sys.argv) == 4:
model_name = sys.argv[1]
target_name = sys.argv[2]
target_func = load_target_func(target_name)
num_steps = int(sys.argv[3])
if model_name in ['pd', 'pid', 'pidt', 'pidtf']:
# The controller does not use a Nengo Model
if model_name == 'pd':
cont = PD(target_func=target_func)
elif model_name == 'pid':
cont = PID(target_func=target_func)
elif model_name == 'pidt':
cont = PIDt(target_func=target_func)
elif model_name == 'pidtf':
cont = PIDt(fast_i=True, target_func=target_func)
else:
cont = PIDt(target_func=target_func)
state = []
count = 0
# if no target is given, run 'forever'
# also don't bother saving anything
if not recording:
while True:
cont.control_step()
else:
handler = logging.StreamHandler()
formatter = logging.Formatter('%(name)-12s %(levelname)5s: %(message)s')
handler.setFormatter(formatter)
root_logger.addHandler(handler)
working = True
def sigterm_handler(signum, frame):
global working
working = False
root_logger.info("Exiting due to SIGTERM")
signal.signal(signal.SIGTERM, sigterm_handler)
with PID(proportional = args.proportional, integrative = args.integrative, derivative = args.derivative,
integral_minimum = args.integral_minimum, integral_maximum = args.integral_maximum, minimal_control = args.minimal_control, force_shutdown_error = args.force_shutdown_error) as controller:
with TargetThermometer(minimal = args.minimal_temperature, maximal = args.maximal_temperature) as element:
with TargetDriver() as driver:
with ClosedLoop(controller = controller, element = element, driver = driver) as system:
while working:
try:
system.step()
time.sleep(args.update_delay)
except KeyboardInterrupt:
break
logging.shutdown()
# uncertainty for prediction
Q = np.eye(4) * 10000
# covariant noise
N = np.random.randint(1, 100, size=(4, 4))
np.fill_diagonal(N, 0)
Q = Q + N
# uncertainty for measurements
R = np.eye(4) * 5
xp_client = XPlaneConnect()
end_x, end_z = runway['terminate_X'], runway['terminate_Z']
origin_x, origin_z = runway['origin_X'], runway['origin_Z']
starting_elevation = runway['starting_elevation']
throttle_controller = PID(2.0, 0.0, 1.0, 10.0, config['sample_time'])
rudder_controller = PID(0.3, 0.4, 1.5, 10.0, config['sample_time'])
num_samples = config['environment_samples']
ws_bound = config['windspeed_bounds']
wh_bound = config['wind_heading_bounds']
center_line = np.array([end_x - origin_x, end_z - origin_z])
kf = KFilter(F, B, Q, H, R)
no_sim_runs = 60
choose_num_samples = [0, 5, 8]
takeoff_successes = [0, 0, 0]
log = open("sample_0.csv", "w")
xp_client.sendDREFs(XPlaneDefs.position_dref, [origin_x, start_y, origin_z])
# want to end here at t = sim_time
desired_x -= origin_x
desired_z -= origin_z
time.sleep(1)
xp_client.sendCOMM("sim/operation/fix_all_systems")
# release park brake
xp_client.sendDREF("sim/flightmodel/controls/parkbrake", 0)
time.sleep(1)
controls = None
throttle_controller = PID(2.0, 0.0, 1.0, 10.0, sample_time)
rudder_controller = PID(0.3, 0.4, 1.5, 10.0, sample_time)
cle = 0
start = time.time()
for t in range(int(simulation_steps // receding_horizon)):
if time.time() - start > 1:
wind_speed = np.random.randint(ws_bins[0], ws_bins[1])
wind_degrees = np.random.randint(wh_bins[0], wh_bins[1])
wind_direction = runway_heading + wind_degrees
xp_wind_direction = -1 * wind_degrees + runway_heading # since positive rotation to right
xp_wind_direction += 180 # wind is counter clockwise of true north
if len(sys.argv) == 2:
model_name = sys.argv[1] #diff model
if len(sys.argv) == 3:
model_name = sys.argv[1]
control_name = sys.argv[2] #diff control method 1 key 2 gesture 3 ??
print('control', control_name, 'model', model_name)
if model_name in ['pd', 'pid', 'pidt']:
if model_name == 'pd':
control = PD(target_func = target_func)
elif model_name == 'pid':
control = PIDt(target_func = target_func)
else:
control = PID(target_func = target_func)
state = []
count = 0
if control_name == 'key':
key_cont.start()
obj = key_cont
print("key board control")
elif control_name == 'hand':
obj = gesture_cont
elif control_name == 'control':
obj = server_cont
while True:
control.control_step(obj, control_name)
while done is False:
a = controller.calculate(s, 0.02)
s_, reward, done = env.step(a)
# add record data
if episode % record_period == 0:
episode_record.add(np.hstack((s, a, [reward], s_))) # add data
s = s_
step += 1
if done:
# record
if episode % record_period == 0:
episode_record.save() # save data
if __name__ == '__main__':
path = "/data/control/" + getTime()
record_path = path + "/record/"
create_dir(path)
PID_controller = PID(kp=0.5, ki=0, kd=0.1, dss_bound=10)
##### select env #################
# track_env = Env(is_bldc2_control=False,is_compare=True) # for random cruve compare
# track_env = Env(is_bldc2_control=True,is_compare=True) # for random motor compare
# track_env = Env(is_bldc2_control=False,is_compare=False) # for random curve single control
track_env = Env(is_bldc2_control=True,
is_compare=False) # for random motor single control
run_task(track_env, PID_controller, 500, record_path=record_path)
s2.Kp = 5 s2.Ki = 16 s2.Kd = 3 s2.Ts = 0.002 # they all must be the same!!! s2.Tt = 1 / s1.Ki s2.YMin = 0 s2.YMax = 10 # PID parameters using each individual setting params = PIDParameters() params.setSettings([s1, s2]) params.wrap = False params.saturate = True params.integration_method = IntegrationMethod.TUSTIN # configure PID instance controller = PID() controller.configParameters(params) # ... code is running ... # ... inside a mainloop to update the controller ... ctrl_in = PIDInput() ctrl_in.time = LinearSystem.getTimeFromSeconds(0.2) ctrl_in.ref = [0.1, 0.2] ctrl_in.sig = [0.3, 0.4] ctrl_in.dref = [0, 0] ctrl_in.dsig = [0, 0] ctrl_in.sat = [0, 0] # last PID output after saturation out = controller.update(ctrl_in) print(out.getValue())
