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