def ask_for_ab_points(gps: adapters.GPSUbloxAdapter): """Ask user for moving vector AB points""" input("Press enter to save point B") point_b = gps.get_fresh_position() print("Point B saved.") input("Press enter to save point A") point_a = gps.get_fresh_position() print("Point A saved.") return [point_a, point_b]
def average_point(gps: adapters.GPSUbloxAdapter, trajectory_saver: TrajectorySaver, nav: navigation.GPSComputing, logger: Logger = None): #ORIGIN POINT SAVING lat = [] #latitude history long = [] #longitude history distances = [] for i in range(0, config.ORIGIN_AVERAGE_SAMPLES): prev_maneuver_time = time.time() try: prev_pos = gps.get_fresh_position() msg = f"Get {i+1}/{config.ORIGIN_AVERAGE_SAMPLES} point in {time.time()-prev_maneuver_time} for average_point." if logger is not None: logger.write_and_flush(msg + "\n") if config.VERBOSE: print(msg) except TimeoutError: msg = f"Erro waiting time too long for the {i+1} point in average_point !" if logger is not None: logger.write_and_flush(msg + "\n") if config.VERBOSE: print(msg) raise TimeoutError lat.append(prev_pos[0]) long.append(prev_pos[1]) mu_lat, sigma_lat = mu_sigma(lat) mu_long, sigma_long = mu_sigma(long) distance = nav.get_distance([mu_lat, mu_long, '1'], prev_pos) #print("| ",get_current_time()," | %2.2f"%distance, " | ", prev_pos, "|") distances.append(distance) time.sleep(0.950) mu_distance, sigma_distance = mu_sigma(distances) #print("stat lattitude : \n") mu_lat, sigma_lat = mu_sigma(lat) #distribution_of_values(lat, mu_lat, sigma_lat) #print("stat longitude : \n") mu_long, sigma_long = mu_sigma(long) #distribution_of_values(long, mu_long, sigma_long) #print("stat distance : \n") mu_distance, sigma_distance = mu_sigma(distances) #distribution_of_values(distances, mu_distance, sigma_distance) #print("Average origin point: %2.13f"%mu_lat," ","%2.13f"%mu_long, "standard deviation (mm) %2.2f"%sigma_distance) prev_pos[ 0] = mu_lat #replace the instantaneous value by the average latitude prev_pos[ 1] = mu_long #replace the instantaneous value by the average longitude prev_pos.append("Origin_with_" + str(config.ORIGIN_AVERAGE_SAMPLES) + "_samples") #print("prev_pos syntax : ",prev_pos) #debug if trajectory_saver is not None: trajectory_saver.save_point(prev_pos) return prev_pos
def move_to_point(coords_from_to: list, used_points_history: list, gps: adapters.GPSUbloxAdapter, vesc_engine: adapters.VescAdapter, smoothie: adapters.SmoothieAdapter, logger: utility.Logger, client, nav: navigation.GPSComputing, raw_angles_history: list): """ Moves to given point. :param coords_from_to: :param used_points_history: :param gps: :param vesc_engine: :param smoothie: :param logger: :param client: :param nav: :return: :param raw_angles_history: """ raw_angles_history = [] stop_helping_point = nav.get_coordinate(coords_from_to[1], coords_from_to[0], 90, 1000) prev_maneuver_time = time.time() prev_point = gps.get_fresh_position( ) # TODO: maybe it's ok to get last position instead of waiting for fresh vesc_engine.set_rpm(int(config.VESC_RPM / 2)) vesc_engine.start_moving() # main navigation control loop while True: cur_pos = gps.get_fresh_position() used_points_history.append(cur_pos.copy()) if not client.sendData("{};{}".format(cur_pos[0], cur_pos[1])): msg = "[Client] Connection closed !" print(msg) logger.write(msg + "\n") if str(cur_pos) == str(prev_point): msg = "Got the same position, added to history, calculations skipped" print(msg) logger.write(msg + "\n") continue distance = nav.get_distance(cur_pos, coords_from_to[1]) msg = "Distance to B: " + str(distance) print(msg) logger.write(msg + "\n") # check if arrived _, side = nav.get_deviation(coords_from_to[1], stop_helping_point, cur_pos) # if distance <= config.COURSE_DESTINATION_DIFF: # old way if side != 1: # TODO: maybe should use both side and distance checking methods at once vesc_engine.stop_moving() # msg = "Arrived (allowed destination distance difference " + str(config.COURSE_DESTINATION_DIFF) + " mm)" msg = "Arrived." print(msg) logger.write(msg + "\n") break # reduce speed if near the target point if config.USE_SPEED_LIMIT: distance_from_start = nav.get_distance(coords_from_to[0], cur_pos) if distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD: vesc_engine.apply_rpm(int(config.VESC_RPM / 2)) else: vesc_engine.apply_rpm(config.VESC_RPM) # do maneuvers not more often than specified value cur_time = time.time() if cur_time - prev_maneuver_time < config.MANEUVERS_FREQUENCY: continue prev_maneuver_time = cur_time msg = "Timestamp: " + str(cur_time) print(msg) logger.write(msg + "\n") msg = "Prev: " + str(prev_point) + " Cur: " + str(cur_pos) + " A: " + str(coords_from_to[0]) \ + " B: " + str(coords_from_to[1]) print(msg) logger.write(msg + "\n") raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos, coords_from_to[1]) # sum(e) if len(raw_angles_history) >= config.WINDOW: raw_angles_history.pop(0) raw_angles_history.append(raw_angle) sum_angles = sum(raw_angles_history) if sum_angles > config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is bigger than max allowed value " + \ str(config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(config.SUM_ANGLES_HISTORY_MAX) print(msg) logger.write(msg) sum_angles = config.SUM_ANGLES_HISTORY_MAX elif sum_angles < -config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is less than min allowed value " + \ str(-config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(-config.SUM_ANGLES_HISTORY_MAX) print(msg) logger.write(msg) sum_angles = -config.SUM_ANGLES_HISTORY_MAX angle_kp_ki = raw_angle * config.KP + sum_angles * config.KI target_angle_sm = angle_kp_ki * -config.A_ONE_DEGREE_IN_SMOOTHIE # smoothie -Value == left, Value == right ad_wheels_pos = smoothie.get_adapter_current_coordinates()["A"] sm_wheels_pos = smoothie.get_smoothie_current_coordinates()["A"] # compute order angle (smoothie can't turn for huge values immediately also as cancel movement, # so we need to do nav. actions in steps) order_angle_sm = target_angle_sm - ad_wheels_pos # check for out of update frequency and smoothie execution speed (for nav wheels) if order_angle_sm > config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE: msg = "Order angle changed from " + str( order_angle_sm) + " to " + str( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND + config.A_ONE_DEGREE_IN_SMOOTHIE ) + " due to exceeding degrees per tick allowed range." print(msg) logger.write(msg + "\n") order_angle_sm = config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE elif order_angle_sm < -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE): msg = "Order angle changed from " + str( order_angle_sm) + " to " + str(-( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE )) + " due to exceeding degrees per tick allowed range." print(msg) logger.write(msg + "\n") order_angle_sm = -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE) # convert to global coordinates order_angle_sm += ad_wheels_pos # checking for out of smoothie supported range if order_angle_sm > config.A_MAX: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MAX = " + \ str(config.A_MAX) + " due to exceeding smoothie allowed values range." print(msg) logger.write(msg + "\n") order_angle_sm = config.A_MAX elif order_angle_sm < config.A_MIN: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MIN = " + \ str(config.A_MIN) + " due to exceeding smoothie allowed values range." print(msg) logger.write(msg + "\n") order_angle_sm = config.A_MIN msg = "Adapter wheels pos (target): " + str(ad_wheels_pos) + " Smoothie wheels pos (current): " \ + str(sm_wheels_pos) print(msg) logger.write(msg + "\n") msg = "KI: " + str(config.KI) + " Sum angles: " + str(sum_angles) + " Sum angles history: " + \ str(raw_angles_history) print(msg) logger.write(msg + "\n") msg = "KP: " + str(config.KP) + " Raw angle: " + str(raw_angle) + " Angle * KP + sum(angles) * KI: " + \ str(angle_kp_ki) + " Smoothie target angle: " + str(target_angle_sm) + \ " Smoothie absolute order angle: " + str(order_angle_sm) print(msg) logger.write(msg + "\n") prev_point = cur_pos response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX) msg = "Smoothie response: " + response print(msg) logger.write(msg) # next tick indent print() logger.write("\n")
def move_to_point( coords_from_to: list, used_points_history: list, gps: adapters.GPSUbloxAdapter, vesc_engine: adapters.VescAdapter, smoothie: adapters.SmoothieAdapter, logger_full: utility.Logger, client, nav: navigation.GPSComputing, raw_angles_history: list, report_writer, report_field_names, logger_table: utility.Logger, periphery_detector: detection.YoloOpenCVDetection, camera: adapters.CameraAdapterIMX219_170, working_zone_points_cv): """ Moves to given point. :param coords_from_to: :param used_points_history: :param gps: :param vesc_engine: :param smoothie: :param logger_full: :param client: :param nav: :param raw_angles_history: :return: """ raw_angles_history = [] stop_helping_point = nav.get_coordinate(coords_from_to[1], coords_from_to[0], 90, 1000) prev_maneuver_time = time.time() prev_point = gps.get_fresh_position( ) # TODO: maybe it's ok to get last position instead of waiting for fresh vesc_engine.set_rpm(config.VESC_RPM_SLOW) vesc_engine.start_moving() # main navigation control loop while True: # DETECTION frame = camera.get_image() plants_boxes = periphery_detector.detect(frame) if len(plants_boxes) > 0: if config.SAVE_DEBUG_IMAGES: debug_save_image(config.DEBUG_IMAGES_PATH, "", frame, plants_boxes, config.UNDISTORTED_ZONE_RADIUS, working_zone_points_cv) # NAVIGATION cur_pos = gps.get_fresh_position() used_points_history.append(cur_pos.copy()) if not client.sendData("{};{}".format(cur_pos[0], cur_pos[1])): msg = "[Client] Connection closed !" print(msg) logger_full.write(msg + "\n") if str(cur_pos) == str(prev_point): msg = "Got the same position, added to history, calculations skipped. Am I stuck?" print(msg) logger_full.write(msg + "\n") continue distance = nav.get_distance(cur_pos, coords_from_to[1]) msg = "Distance to B: " + str(distance) # print(msg) logger_full.write(msg + "\n") # check if arrived _, side = nav.get_deviation(coords_from_to[1], stop_helping_point, cur_pos) # if distance <= config.COURSE_DESTINATION_DIFF: # old way if side != 1: # TODO: maybe should use both side and distance checking methods at once vesc_engine.stop_moving() # msg = "Arrived (allowed destination distance difference " + str(config.COURSE_DESTINATION_DIFF) + " mm)" msg = "Arrived to " + str(coords_from_to[1]) # print(msg) logger_full.write(msg + "\n") break # reduce speed if near the target point if config.USE_SPEED_LIMIT: distance_from_start = nav.get_distance(coords_from_to[0], cur_pos) if distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD: vesc_engine.apply_rpm(config.VESC_RPM_SLOW) else: vesc_engine.apply_rpm(config.VESC_RPM_FAST) # do maneuvers not more often than specified value cur_time = time.time() if cur_time - prev_maneuver_time < config.MANEUVERS_FREQUENCY: continue prev_maneuver_time = cur_time msg = "Timestamp: " + str(cur_time) # print(msg) logger_full.write(msg + "\n") msg = "Prev: " + str(prev_point) + " Cur: " + str(cur_pos) + " A: " + str(coords_from_to[0]) \ + " B: " + str(coords_from_to[1]) # print(msg) logger_full.write(msg + "\n") raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos, coords_from_to[1]) # sum(e) if len(raw_angles_history) >= config.WINDOW: raw_angles_history.pop(0) raw_angles_history.append(raw_angle) sum_angles = sum(raw_angles_history) if sum_angles > config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is bigger than max allowed value " + \ str(config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(config.SUM_ANGLES_HISTORY_MAX) # print(msg) logger_full.write(msg + "\n") sum_angles = config.SUM_ANGLES_HISTORY_MAX elif sum_angles < -config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is less than min allowed value " + \ str(-config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(-config.SUM_ANGLES_HISTORY_MAX) # print(msg) logger_full.write(msg + "\n") sum_angles = -config.SUM_ANGLES_HISTORY_MAX angle_kp_ki = raw_angle * config.KP + sum_angles * config.KI target_angle_sm = angle_kp_ki * -config.A_ONE_DEGREE_IN_SMOOTHIE # smoothie -Value == left, Value == right ad_wheels_pos = smoothie.get_adapter_current_coordinates()["A"] sm_wheels_pos = smoothie.get_smoothie_current_coordinates()["A"] # compute order angle (smoothie can't turn for huge values immediately also as cancel movement, # so we need to do nav. actions in steps) order_angle_sm = target_angle_sm - ad_wheels_pos # check for out of update frequency and smoothie execution speed range (for nav wheels) if order_angle_sm > config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE: msg = "Order angle changed from " + str( order_angle_sm) + " to " + str( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND + config.A_ONE_DEGREE_IN_SMOOTHIE ) + " due to exceeding degrees per tick allowed range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE elif order_angle_sm < -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE): msg = "Order angle changed from " + str( order_angle_sm) + " to " + str(-( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE )) + " due to exceeding degrees per tick allowed range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE) # convert to global smoothie coordinates order_angle_sm += ad_wheels_pos # checking for out of smoothie supported range if order_angle_sm > config.A_MAX: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MAX = " + \ str(config.A_MAX) + " due to exceeding smoothie allowed values range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.A_MAX elif order_angle_sm < config.A_MIN: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MIN = " + \ str(config.A_MIN) + " due to exceeding smoothie allowed values range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.A_MIN raw_angle = round(raw_angle, 2) angle_kp_ki = round(angle_kp_ki, 2) order_angle_sm = round(order_angle_sm, 2) sum_angles = round(sum_angles, 2) distance = round(distance, 2) ad_wheels_pos = round(ad_wheels_pos, 2) sm_wheels_pos = round(sm_wheels_pos, 2) gps_quality = cur_pos[2] msg = str(gps_quality).ljust(5) + str(raw_angle).ljust(8) + str( angle_kp_ki).ljust(8) + str(order_angle_sm).ljust(8) + str( sum_angles).ljust(8) + str(distance).ljust(13) + str( ad_wheels_pos).ljust(8) + str(sm_wheels_pos).ljust(9) print(msg) logger_full.write(msg + "\n") # load sensors data to csv s = "," msg = str(gps_quality) + s + str(raw_angle) + s + str(angle_kp_ki) + s + str(order_angle_sm) + s + \ str(sum_angles) + s + str(distance) + s + str(ad_wheels_pos) + s + str(sm_wheels_pos) vesc_data = vesc_engine.get_sensors_data(report_field_names) if vesc_data is not None: msg += s for key in vesc_data: msg += str(vesc_data[key]) + s msg = msg[:-1] logger_table.write(msg + "\n") prev_point = cur_pos response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX) if response != smoothie.RESPONSE_OK: msg = "Smoothie response is not ok: " + response + "\n" print(msg) logger_full.write(msg + "\n\n")