def __init__(self):
# control algorithms
self.pid_camera_params = {'kp':1.5, 'kd':3.0, 'kd2':2.0, 'error':0.0, 'last_error':0.0, 'next_error':0.0 }
self.pid_wall_following = my_controller.pid(kp=0.0033, ki=0.0003, kd=0.1, integral_max=1000, output_max=1.0, alpha=0.1)
self.pid_speed = my_controller.pid(kp=70.0, ki=0.0, kd=100.0, integral_max=1000, output_max=50.0, alpha=0.5)
# controller settings
self.pid_speed_kff = 0.0 # feed forward apart from PID
self.min_speed = 0.5 # m/s
self.max_speed = 2.0 # m/s
self.k_speed = 0.01
self.positional_error_threshold = 350
self.dual_rate = 0.5
self.direction_trim = 0
# controller state
self.direction = 128
self.throttle = 128
self.mode = 0
self.actual_error = 0.0
self.pid_wall = 0.0
self.pid_ai = 0.0
self.positional_error = 0.0
self.actual_speed_error_ms = 0.0 # m/s
self.target_speed_ms = 0.0 # m/s
# controller inputs data from STM32 controller board
self.lidar_distance_gauche = 0
self.lidar_distance_droit = 0
self.lidar_distance_haut = 0
self.telemetry_speed = 0
self.telemetry_manual_dir = 0
self.telemetry_manual_thr = 0
self.telemetry_auto_dir = 0
self.telemetry_auto_thr = 0
self.telemetry_start_button = 0
self.actual_speed_ms = 0.0
self.actual_speed_kmh = 0.0
# serial port for STM32 data link
self.port = serial.Serial("/dev/ttyUSB0",baudrate=115200,timeout=3.0)
# global state
self.ia = False # control CNN and image processing
self.running = False # control THROTTLE !!!
self.recording = False # control recording video and data locally
self.dataset = 0 # control recording of a dataset (picture,DIR,THR)
# camera handler
self.video_capture = 0
self.video_broadcast = 0
self.video_recorder = 0
self.video_broadcast_IP_address ='10.42.0.112' # IPv4 address of PC connectd through WiFi and receiving video stream
# camera settings
self.rec_width = 1280
self.rec_height = 720
self.new_width = 160
self.new_height = 90
# camera state
self.image = 0
self.init_video()
# AI handler
self.model = 0
# AI settings
self.ia_version = 2
# AI state
self.y_1 = 0
self.y_2 = 0
# Data logger
self.datalogger = my_datalogger.datalogger("datalogger")
# Dataset
self.dataset_directory = "dataset"
self.dataset_counter = 0
self.dataset_file = None
def main():
global remote_configuration_command_id
# FPS debug
last_time = time.time()
frame_counter = 0
fps = 0
fps_counter = 0
# Load default settings
load_configuration_file()
# Controller start
controller = main_controller()
controller.reload_settings()
# Servers start
remote_configuration_server("10.42.0.1", 6000)
tserver = telemetry_server("10.42.0.1", 7001)
# Data logger
datalogger = my_datalogger.datalogger("datalogger")
# Game loop
counter = 0
while True:
# Non blocking call
asyncore.loop(timeout=0, count=1)
# Parse and execute remote commands
if remote_configuration_command_id == 1:
controller.quit()
break
elif remote_configuration_command_id == 2:
controller.init_ai()
elif remote_configuration_command_id == 3:
print("Start recording...")
controller.start_recording()
elif remote_configuration_command_id == 4:
print("Stop recording!")
controller.stop_recording()
elif remote_configuration_command_id == 5:
print("Start running...")
controller.start_running()
datalogger.start()
elif remote_configuration_command_id == 6:
print("Stop running!")
datalogger.stop()
controller.stop_running()
elif remote_configuration_command_id == 10:
controller.reload_settings()
remote_configuration_command_id = 0 # reset
# Process
controller.process()
# Telemetry
counter += 1
msg = str(counter) + ';'
msg += str(float(controller.lidar_distance_gauche)) + ';' #cm
msg += str(float(controller.lidar_distance_droit)) + ';' #cm
msg += str(float(controller.lidar_distance_haut)) + ';' #cm
msg += str(float(controller.actual_error)) + ';' #cm lidar error
msg += str(float(
controller.positional_error)) + ';' #cm lidar error integral
msg += str(float(controller.pid_wall)) + ';'
msg += str(float(controller.pid_ai)) + ';'
msg += str(float(controller.direction)) + ';'
msg += str(float(controller.throttle)) + ';'
msg += str(float(controller.mode))
msg_length = str(len(msg)).ljust(4)
tserver.sendTelemetry(msg_length)
tserver.sendTelemetry(msg)
# Data logger
log_list = []
log_list.append(float(fps))
log_list.append(float(controller.lidar_distance_gauche))
log_list.append(float(controller.lidar_distance_droit))
log_list.append(float(controller.lidar_distance_haut))
log_list.append(float(controller.actual_error))
log_list.append(float(controller.positional_error))
log_list.append(float(controller.pid_wall))
log_list.append(float(controller.pid_ai))
log_list.append(float(controller.direction))
log_list.append(float(controller.throttle))
log_list.append(float(controller.mode))
datalogger.record(log_list)
# FPS debug
frame_counter += 1
if time.time() >= last_time + 1.0:
last_time = time.time()
fps = frame_counter
frame_counter = 0
# Local trace
if fps_counter % 60 == 0:
print("fps:" + str(fps) + " DIR:" + str(controller.direction) +
" THR:" + str(controller.throttle) + " MODE:" +
str(controller.mode) + " LiG:" +
str(controller.lidar_distance_gauche) + " LiD:" +
str(controller.lidar_distance_droit) + " LiH:" +
str(controller.lidar_distance_haut))
fps_counter += 1
def main():
global command_id
global telemetryClient
# FPS debug
last_time = time.time()
frame_counter = 0
fps = 0
fps_counter = 0
# Settings
command_id = 0 # idle
load_configuration_file()
# Server start
serv = buggyServer("10.42.0.1", 6000)
serv.reload_settings()
serverTelemetry = buggyTelemetryServer("10.42.0.1", 7001)
telemetryClient = False
counter = 0
# Data logger
datalogger = my_datalogger.datalogger("datalogger")
# Main loop
while True:
# Non blocking call
asyncore.loop(timeout=0, count=1)
# Process command ID
if command_id == 1:
serv.quit()
break
elif command_id == 2:
serv.init_IA()
elif command_id == 3:
print("Start recording...")
#serv.xxxx()
elif command_id == 4:
print("Stop recording!")
#serv.xxxx()
elif command_id == 5:
print("Start running...")
serv.start_running()
datalogger.start()
elif command_id == 6:
print("Stop running!")
datalogger.stop()
serv.stop_running()
elif command_id == 10:
serv.reload_settings()
command_id = 0
# Process
serv.process()
# Telemetry
counter += 1
msg = str(counter) + ';' + str(float(serv.direction)) + ';' + str(
float(serv.throttle)) + ';' + str(float(serv.actual_error))
serverTelemetry.sendTelemetry(msg)
# Data logger
log_list = []
log_list.append(float(fps))
log_list.append(float(serv.lidar_distance_gauche))
log_list.append(float(serv.lidar_distance_droit))
log_list.append(float(serv.actual_error))
log_list.append(float(serv.positional_error))
log_list.append(float(serv.pid_wall))
log_list.append(float(serv.direction))
log_list.append(float(serv.throttle))
log_list.append(float(serv.mode))
datalogger.record(log_list)
# FPS debug
frame_counter += 1
if time.time() >= last_time + 1.0:
last_time = time.time()
fps = frame_counter
frame_counter = 0
# Local Debug
if fps_counter % 60 == 0:
print("fps:" + str(fps) + " DIR:" + str(serv.direction) +
" THR:" + str(serv.throttle) + " MODE:" +
str(serv.mode) + " LiG:" +
str(serv.lidar_distance_gauche) + " LiD:" +
str(serv.lidar_distance_droit))
fps_counter += 1