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