Example #1
0
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]
Example #2
0
def ask_for_ab_points(gps: adapters.GPSUbloxAdapter):
    """Ask user for moving vector AB points"""

    # TO DO: remove time.sleep when blocking gps.get_fresh_position function will be done
    sleep_time = 1
    # wait for gps
    while gps.get_stored_pos_count() == 0:
        time.sleep(0.05)

    input("Press enter to save point B")
    time.sleep(sleep_time)
    point_b = gps.get_last_position()
    print("Point B saved.")
    input("Press enter to save point A")
    time.sleep(sleep_time)
    point_a = gps.get_last_position()
    print("Point A saved.")
    return [point_a, point_b]
Example #3
0
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
Example #4
0
def move_to_point_and_extract(
        coords_from_to: list, gps: adapters.GPSUbloxAdapter,
        vesc_engine: adapters.VescAdapter, smoothie: adapters.SmoothieAdapter,
        camera: adapters.CameraAdapterIMX219_170,
        periphery_det: detection.YoloOpenCVDetection,
        precise_det: detection.YoloOpenCVDetection, client,
        logger_full: utility.Logger, logger_table: utility.Logger,
        report_field_names, used_points_history: list, undistorted_zone_radius,
        working_zone_polygon, working_zone_points_cv, view_zone_polygon,
        view_zone_points_cv, img_output_dir, nav: navigation.GPSComputing):
    """
    Moves to the given target point and extracts all weeds on the way.
    :param coords_from_to:
    :param gps:
    :param vesc_engine:
    :param smoothie:
    :param camera:
    :param periphery_det:
    :param precise_det:
    :param client:
    :param logger_full:
    :param logger_table:
    :param report_field_names:
    :param used_points_history:
    :param nav:
    :param working_zone_polygon:
    :param undistorted_zone_radius:
    :param working_zone_points_cv:
    :param img_output_dir:
    :return:
    """

    vesc_engine.apply_rpm(config.VESC_RPM_SLOW)
    slow_mode_time = -float("inf")
    current_working_mode = working_mode_slow = 1
    working_mode_switching = 2
    working_mode_fast = 3
    close_to_end = True  # True if robot is close to one of current movement vector points, False otherwise

    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_pos = gps.get_last_position()

    # set camera to the Y min
    res = smoothie.custom_move_to(config.XY_F_MAX,
                                  X=config.X_MAX / 2 /
                                  config.XY_COEFFICIENT_TO_MM,
                                  Y=config.Y_MIN)
    if res != smoothie.RESPONSE_OK:
        msg = "INIT: Failed to move camera to Y min, smoothie response:\n" + res
        logger_full.write(msg + "\n")
    smoothie.wait_for_all_actions_done()

    # main navigation control loop
    while True:
        # EXTRACTION CONTROL
        start_t = time.time()
        frame = camera.get_image()
        frame_t = time.time()

        plants_boxes = periphery_det.detect(frame)
        per_det_t = time.time()

        debug_save_image(
            img_output_dir,
            "(periphery view scan M=" + str(current_working_mode) + ")", frame,
            plants_boxes, undistorted_zone_radius, working_zone_points_cv
            if current_working_mode == 1 else view_zone_points_cv)
        msg = "View frame time: " + str(
            frame_t - start_t) + "\t\tPer. det. time: " + str(per_det_t -
                                                              frame_t)
        logger_full.write(msg + "\n")

        # slow mode
        if current_working_mode == working_mode_slow:
            if any_plant_in_zone(plants_boxes, working_zone_polygon):
                vesc_engine.stop_moving()
                time.sleep(0.2)

                start_work_t = time.time()
                frame = camera.get_image()
                frame_t = time.time()

                plants_boxes = precise_det.detect(frame)
                pre_det_t = time.time()

                debug_save_image(img_output_dir, "(precise view scan M=1)",
                                 frame, plants_boxes, undistorted_zone_radius,
                                 working_zone_points_cv)
                msg = "Work frame time: " + str(
                    frame_t -
                    start_work_t) + "\t\tPrec. det. time: " + str(pre_det_t -
                                                                  frame_t)
                logger_full.write(msg + "\n")

                if any_plant_in_zone(plants_boxes, working_zone_polygon):
                    extract_all_plants(smoothie, camera, precise_det,
                                       working_zone_polygon, frame,
                                       plants_boxes, undistorted_zone_radius,
                                       working_zone_points_cv, img_output_dir)
            elif not any_plant_in_zone(plants_boxes, view_zone_polygon) and \
                    time.time() - slow_mode_time > config.SLOW_MODE_MIN_TIME:
                # set camera to the Y max
                res = smoothie.custom_move_to(
                    config.XY_F_MAX,
                    X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM,
                    Y=config.Y_MAX / config.XY_COEFFICIENT_TO_MM)
                if res != smoothie.RESPONSE_OK:
                    msg = "M=" + str(
                        current_working_mode
                    ) + ": " + "Failed to move to Y max, smoothie response:\n" + res
                    logger_full.write(msg + "\n")
                smoothie.wait_for_all_actions_done()
                current_working_mode = working_mode_switching
            vesc_engine.start_moving()

        # switching to fast mode
        elif current_working_mode == working_mode_switching:
            if any_plant_in_zone(plants_boxes, view_zone_polygon):
                vesc_engine.stop_moving()
                # set camera to the Y min
                res = smoothie.custom_move_to(config.XY_F_MAX,
                                              X=config.X_MAX / 2 /
                                              config.XY_COEFFICIENT_TO_MM,
                                              Y=config.Y_MIN)
                if res != smoothie.RESPONSE_OK:
                    msg = "M=" + str(
                        current_working_mode
                    ) + ": " + "Failed to move to Y min, smoothie response:\n" + res
                    logger_full.write(msg + "\n")
                smoothie.wait_for_all_actions_done()
                current_working_mode = working_mode_slow
                slow_mode_time = time.time()
            elif smoothie.get_smoothie_current_coordinates(False)[
                    "Y"] + config.XY_COEFFICIENT_TO_MM * 20 > config.Y_MAX:
                current_working_mode = working_mode_fast
                if not close_to_end:
                    vesc_engine.apply_rpm(config.VESC_RPM_FAST)

        # fast mode
        else:
            if any_plant_in_zone(plants_boxes, view_zone_polygon):
                vesc_engine.stop_moving()
                # set camera to the Y min
                res = smoothie.custom_move_to(config.XY_F_MAX,
                                              X=config.X_MAX / 2 /
                                              config.XY_COEFFICIENT_TO_MM,
                                              Y=config.Y_MIN)
                if res != smoothie.RESPONSE_OK:
                    msg = "M=" + str(
                        current_working_mode
                    ) + ": " + "Failed to move to Y min, smoothie response:\n" + res
                    logger_full.write(msg + "\n")
                smoothie.wait_for_all_actions_done()
                current_working_mode = working_mode_slow
                slow_mode_time = time.time()
                vesc_engine.apply_rpm(config.VESC_RPM_SLOW)
            elif close_to_end:
                vesc_engine.apply_rpm(config.VESC_RPM_SLOW)
            else:
                vesc_engine.apply_rpm(config.VESC_RPM_FAST)

        # NAVIGATION CONTROL
        nav_start_t = time.time()
        cur_pos = gps.get_last_position()

        if str(cur_pos) == str(prev_pos):
            # msg = "Got the same position, added to history, calculations skipped. Am I stuck?"
            # print(msg)
            # logger_full.write(msg + "\n")
            continue

        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")

        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

        # pass by cur points which are very close to prev point to prevent angle errors when robot is staying
        # (too close points in the same position can produce false huge angles)
        if nav.get_distance(prev_pos,
                            cur_pos) < config.PREV_CUR_POINT_MIN_DIST:
            continue

        # reduce speed if near the target point
        if config.USE_SPEED_LIMIT:
            distance_from_start = nav.get_distance(coords_from_to[0], cur_pos)
            close_to_end = distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD

        # 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 = "Prev: " + str(prev_pos) + " 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_pos, 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_pos = cur_pos
        response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX)

        if response != smoothie.RESPONSE_OK:  # TODO: what if response is not ok?
            msg = "Smoothie response is not ok: " + response
            print(msg)
            logger_full.write(msg + "\n")

        msg = "Nav calc time: " + str(time.time() - nav_start_t)
        logger_full.write(msg + "\n\n")
Example #5
0
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")
Example #6
0
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")