def control_loop(self, vr: Any, car: Car,
csvfile: Optional[SimpleCsvDictWriter]) -> None:
"""Control iteration. This is called on a regular basis.
Args:
vr -- VRepInterface object, which is an abstraction on top of the VREP
Python API. See the class docstring near the top of this file.
car -- Car object, providing abstractions for the car (like steering and
velocity control).
csvfile -- Optional to log data. None to disable.
"""
sim_time = car.get_sim_time()
dt = sim_time - self.last_sim_time
self.last_sim_time = sim_time
#
# ASSIGNMENT: Tune / implement a better controller loop here.
#
# Here are two different sensors you can play with.
# One provides an ideal shortest-distance-to-path (in meters), but isn't
# path-following and may jump at crossings.
# The other uses a more realistic line sensor (in pixels), which you can
# plug your track detection algorithm into.
line_camera_image0 = car.get_line_camera_image(0)
# line0_err = self.get_line_camera_error(car.get_line_camera_image(0))
line0_err = self.get_line_camera_error(self.oldimage,
car.get_line_camera_image(0),
self.old_lat_err)
#line1_err = self.get_line_camera_error(line_camera_image1, car.get_line_camera_image(1), car.old_lat_err)
line1_err = 0
self.oldimage = line_camera_image0
# line camera has 0.7 m field of view
lat_err = -(np.float(line0_err) /
128) * 0.7 # pixel to meter conversion
if dt > 0.0:
lat_vel = (lat_err - self.old_lat_err) / dt
else:
lat_vel = 0.0
self.old_lat_err = lat_err
# calculate integral error
self.int_err = self.int_err + dt * lat_err
# Proportional gain in steering control (degrees) / lateral error (meters)
kp = 195
kd = 0.5 * kp # deg per m/s
ki = 0.4 * kp # deg per m-s
steer_angle = -kp * lat_err - kd * lat_vel - ki * self.int_err
steer_angle = car.set_steering(
steer_angle,
dt) # use set_steering to include servo slew rate limit
# steer_angle = car.set_steering_fast(steer_angle,dt) # use set_steering_fast for no delay
# Constant speed for now. You can tune this and/or implement advanced controllers.
car.set_speed(3)
# Print out and record debugging info
pos = car.get_position()
vel_vector = car.get_velocity()
vel = math.sqrt(vel_vector[0]**2 + vel_vector[1]**2 + vel_vector[2]**2)
print(
't=%6.3f (x=%5.2f, y=%5.2f, sp=%5.2f): lat_err=%5.2f, int_err=%5.2f, line0_err=%3i, steer_angle=%3.1f'
% (sim_time, pos[0], pos[1], vel, lat_err, self.int_err,
(line0_err or 0), steer_angle))
if csvfile is not None:
csvfile.writerow({
't': sim_time,
'x': pos[0],
'y': pos[1],
'linescan': line_camera_image0,
'line_pos': line0_err +
63, # needs to be in camera pixels so overlaid plots work
#'linescan_far': line_camera_image1,
'line_pos_far': line1_err + 63,
'speed': vel,
'lat_err': lat_err,
'steer_angle': steer_angle,
})
(completed_laps + 1, car.get_sim_time() - sim_start_time))
lap_start_time = car.get_sim_time()
if args.csvfile and csvfile is not None and not done:
csvfile.close()
csvfile = SimpleCsvDictWriter(args.csvfile + '_lap' + str(completed_laps + 1) + '.csv')
if stopping_wire.check_tripped() and completed_laps > 0:
print("\ncrossed stopping line, lap+%.2f, total elapsed time: %.2f" %
(car.get_sim_time() - lap_start_time, car.get_sim_time() - sim_start_time))
if completed_laps >= args.laps and args.laps != 0:
done = True
if args.synchronous:
vr.simxSynchronousTrigger()
except vrepInterface.VRepAPIError:
print("\n VRepAPIError- simulation stopped. Setting speed 0\n")
car.set_speed(0.0) # set to zero, otherwise car will lunge at next sim start
pass
except KeyboardInterrupt:
# Allow a keyboard interrupt to break out of the loop while still shutting
# down gracefully.
print("\ncaught keyboard interrupt, terminating control loop")
pass
finally:
print("\nending simulation")
vr.simxStopSimulation(vrep.simx_opmode_oneshot_wait)
if csvfile is not None:
csvfile.close() # close file and writer
print("csv files closed")