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