def main():
try:
create_directories(WITH_PLANTS_DIR, WITHOUT_PLANTS_DIR)
working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS)
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
with adapters.CameraAdapterIMX219_170(config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM, config.ISP_DIGITAL_GAIN_RANGE_TO,
config.GAIN_RANGE_FROM, config.GAIN_RANGE_TO,
config.EXPOSURE_TIME_RANGE_FROM, config.EXPOSURE_TIME_RANGE_TO,
config.AE_LOCK, config.CAMERA_W, config.CAMERA_H, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_FRAMERATE,
config.CAMERA_FLIP_METHOD) as camera:
with adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc_engine:
counter = 1
session_label = input("Set session label (added to names): ")
input("Press enter to start.")
print("Data gathering started.")
while True:
counter = gather_data(smoothie, camera, counter, session_label, working_zone_polygon)
# move_forward_smoothie(1100, -5.2, smoothie) # F1100, B-5.2 = 30 cm with max speed (B-104 F1900 for min speed 30 cm) # for smoothie moving forward control
vesc_engine.start_moving()
vesc_engine.wait_for_stop()
except KeyboardInterrupt:
print("Stopped by a keyboard interrupt")
except:
print(traceback.format_exc())
finally:
smoothie.disconnect()
print("Data gathering done.", str(counter - 1), "images collected at this session.")
def main():
nav = navigation.GPSComputing()
with adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP) as gps:
with adapters.VescAdapter(RPM, MOVING_TIME, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT,
config.VESC_BAUDRATE) as vesc_engine:
print("Getting a starting point...")
starting_point = gps.get_last_position()
print("Moving forward...")
vesc_engine.start_moving()
vesc_engine.wait_for_stop()
print("Getting point A...")
time.sleep(1)
A = gps.get_last_position()
print("Computing rest points...")
B = nav.get_coordinate(A, starting_point, 180, FIELD_SIZE)
C = nav.get_coordinate(B, A, 90, FIELD_SIZE)
D = nav.get_coordinate(C, B, 90, FIELD_SIZE)
print("Saving field.txt file...")
save_gps_coordinates([A, B, C, D], "field.txt")
print("Done.")
def initVesc(logger: utility.Logger):
smoothie_vesc_addr = utility.get_smoothie_vesc_addresses()
if "vesc" in smoothie_vesc_addr:
vesc_address = smoothie_vesc_addr["vesc"]
else:
msg = "Couldn't get vesc's USB address!"
logger.write_and_flush(msg+"\n")
print(msg)
exit(1)
vesc_engine = adapters.VescAdapter(0, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, vesc_address, config.VESC_BAUDRATE)
return vesc_engine
def main():
report_field_names = [
'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current',
'avg_input_current', 'rpm', 'input_voltage'
]
print("Loading vesc...")
with adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ,
config.VESC_PORT, config.VESC_BAUDRATE) as vesc:
time.sleep(2)
voltage = vesc.get_sensors_data(report_field_names)["input_voltage"]
print(voltage)
def __th_vesc():
print("Loading vesc...")
with adapters.VescAdapter(config.VESC_RPM_SLOW, float("inf"),
config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE) as vesc_engine:
print("Starting vesc...")
vesc_engine.start_moving()
while KEEP_WORKING:
time.sleep(0.3)
vesc_engine.stop_moving()
print("Vesc is stopped correctly.")
def main():
with adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE) as vesc_engine:
try:
rpm = int(input("Set RPM: "))
moving_time = float(
input(
"Set moving time (seconds; will start moving immediately): "
))
vesc_engine.set_rpm(rpm)
vesc_engine.set_moving_time(moving_time)
vesc_engine.start_moving()
vesc_engine.wait_for_stop()
except KeyboardInterrupt:
print("Emergency stop (KB interrupt)")
vesc_engine.stop_moving()
finally:
vesc_engine.set_moving_time(config.VESC_MOVING_TIME)
vesc_engine.set_rpm(config.VESC_RPM_SLOW)
print("Done.")
def __init__(self):
self.all_tests = 14
self.pass_tests = 0
self.smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
self.vesc = adapters.VescAdapter(-2800, 10, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
self.gps = adapters.GPSUbloxAdapter(config.GPS_PORT,
config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
self.camera = adapters.CameraAdapterIMX219_170(
config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM,
config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM,
config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM,
config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_W, config.CAMERA_H,
config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD)
self.name = datetime.datetime.now().strftime("%d-%m-%Y %H:%M")
self.logger = utility.Logger(self.name + '.txt')
def main():
points_history = []
prev_point = None
try:
nav = navigation.GPSComputing()
print("Initializing...", end=" ")
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
with adapters.VescAdapter(config.VESC_RPM, config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT,
config.VESC_BAUDRATE) as vesc_engine:
with adapters.GPSUbloxAdapter(
config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP) as gps:
print("done.")
field_gps_coords = ask_for_ab_points(gps)
save_gps_coordinates(
field_gps_coords,
"field " + get_current_time() + ".txt")
input("Press enter to start moving")
# start moving forward
vesc_engine.start_moving()
# main navigation control loop
# TO DO: this loop is working much faster than gps, need to evaluate sleep time or waiting for new P
while True:
time.sleep(0.5)
cur_pos = gps.get_last_position()
# if str(cur_pos) == prev_point:
# continue
# prev_point = str(cur_pos)
points_history.append(cur_pos.copy())
# check if arrived
# TO DO: it won't work if deviation > course destination diff, as robot will be far away on some
# side and will never get too close to the path ending point
distance = nav.get_distance(cur_pos,
field_gps_coords[1])
print("Distance to B:", distance)
if distance <= config.COURSE_DESTINATION_DIFF:
vesc_engine.stop_moving()
print(
"Arrived (allowed destination distance difference",
config.COURSE_DESTINATION_DIFF, "mm)")
break
# check for course deviation. if deviation is bigger than a threshold
deviation, side = nav.get_deviation(
field_gps_coords[0], field_gps_coords[1], cur_pos)
nav_wheels_position = smoothie.get_adapter_current_coordinates(
)["A"]
side_text = "(left)" if side == -1 else "(right)" if side == 1 else "(center)"
print("A:", field_gps_coords[0], "B:",
field_gps_coords[1], "Cur:", cur_pos)
print("Deviation:", deviation, "Max dev.:",
config.COURSE_SIDE_DEVIATION_MAX, "side flag:",
side, side_text)
if deviation > config.COURSE_SIDE_DEVIATION_MAX:
# deviation to the left side
if side == -1:
# if not turned wheels yet
# TO DO: try to make different wheels turning values for different deviation
if not (-config.COURSE_ADJ_SMC_VAL - 0.001 <
nav_wheels_position <
-config.COURSE_ADJ_SMC_VAL + 0.001):
# TO DO: check abs(COURSE_ADJ_SMC_VAL - nav_wheels_position) and set bigger or
# lesser turning value
print("Turning wheels left",
-config.COURSE_ADJ_SMC_VAL)
smoothie.nav_turn_wheels_to(
-config.COURSE_ADJ_SMC_VAL,
config.A_F_MAX)
else:
print("Wheels already at left side:",
nav_wheels_position)
# deviation to the right side
elif side == 1:
# if not turned wheels yet
# TO DO: try to make different wheels turning values for different deviation
if not (config.COURSE_ADJ_SMC_VAL - 0.001 <
nav_wheels_position <
config.COURSE_ADJ_SMC_VAL + 0.001):
# TO DO: check abs(COURSE_ADJ_SMC_VAL - nav_wheels_position) and set bigger or
# lesser turning value
print("Turning wheels right:",
config.COURSE_ADJ_SMC_VAL)
smoothie.nav_turn_wheels_to(
config.COURSE_ADJ_SMC_VAL,
config.A_F_MAX)
else:
print("Wheels already at right side:",
nav_wheels_position)
# it's an error if deviation is big but side flag is telling us that we're on moving vector
else:
print("Abnormal state: deviation =", deviation,
"at", side,
"side. Emergency stop applied.")
exit(1)
# if deviation is ok (0 or less than an adj. threshold)
else:
# if not turned wheels yet
# TO DO: try to make different wheels turning values for different deviation
if not (-0.001 < nav_wheels_position < 0.001):
# TO DO: check abs(COURSE_ADJ_SMC_VAL - nav_wheels_position) and set bigger or
# lesser turning value
print("Turning wheels to center")
smoothie.nav_align_wheels_center(
config.A_F_MAX)
else:
print("Wheels already at center:",
nav_wheels_position)
print()
print("Done!")
except KeyboardInterrupt:
print("Stopped by a keyboard interrupt (Ctrl + C)")
finally:
save_gps_coordinates(points_history,
"gps_history " + get_current_time() + ".txt")
def main():
create_directories(config.DEBUG_IMAGES_PATH)
working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS)
working_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
view_zone_polygon = Polygon(config.VIEW_ZONE_POLY_POINTS)
view_zone_points_cv = np.array(config.VIEW_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
nav = navigation.GPSComputing()
used_points_history = []
logger_full = utility.Logger("full log " + get_current_time() + ".txt")
logger_table = utility.Logger("table log " + get_current_time() + ".csv")
# sensors picking
report_field_names = [
'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current',
'avg_input_current', 'rpm', 'input_voltage'
]
# QGIS and sensor data transmitting
path = os.path.abspath(os.getcwd())
sensor_processor = SensorProcessing.SensorProcessing(path, 0)
sensor_processor.startServer()
client = socketForRTK.Client.Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger_full.write(msg + "\n")
sensor_processor.startSession()
try:
msg = "Initializing..."
print(msg)
logger_full.write(msg + "\n")
# load field coordinates
if config.RECEIVE_FIELD_FROM_RTK:
msg = "Loading field coordinates from RTK"
logger_full.write(msg + "\n")
try:
field_gps_coords = load_coordinates(rtk.CURRENT_FIELD_PATH)
except AttributeError:
msg = "Couldn't get field file name from RTK script as it is wasn't assigned there."
print(msg)
logger_full.write(msg + "\n")
exit(1)
except FileNotFoundError:
msg = "Couldn't not find " + rtk.CURRENT_FIELD_PATH + " file."
print(msg)
logger_full.write(msg + "\n")
exit(1)
if len(field_gps_coords) < 5:
msg = "Expected at least 4 gps points in " + rtk.CURRENT_FIELD_PATH + ", got " + \
str(len(field_gps_coords))
print(msg)
logger_full.write(msg + "\n")
exit(1)
field_gps_coords = nav.corner_points(field_gps_coords,
config.FILTER_MAX_DIST,
config.FILTER_MIN_DIST)
else:
msg = "Loading " + config.INPUT_GPS_FIELD_FILE
logger_full.write(msg + "\n")
field_gps_coords = load_coordinates(
config.INPUT_GPS_FIELD_FILE) # [A, B, C, D]
# check field corner points count
if len(field_gps_coords) != 4:
msg = "Expected 4 gps corner points, got " + str(
len(field_gps_coords)) + "\nField:\n" + str(field_gps_coords)
print(msg)
logger_full.write(msg + "\n")
exit(1)
field_gps_coords = reduce_field_size(field_gps_coords,
config.FIELD_REDUCE_SIZE, nav)
# generate path points
path_points = build_path(field_gps_coords, nav, logger_full)
if len(path_points) > 0:
save_gps_coordinates(
path_points, "generated_path " + get_current_time() + ".txt")
else:
msg = "List of path points is empty, saving canceled."
print(msg)
logger_full.write(msg + "\n")
if len(path_points) < 2:
msg = "Expected at least 2 points in path, got " + str(len(path_points)) + \
" instead (1st point is starting point)."
print(msg)
logger_full.write(msg + "\n")
exit(1)
# load and connect to everything
print("Loading periphery detector...")
periphery_detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD,
config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND,
config.PERIPHERY_DNN_TARGET)
print("Loading precise detector...")
precise_detector = detection.YoloOpenCVDetection(
config.PRECISE_CLASSES_FILE, config.PRECISE_CONFIG_FILE,
config.PRECISE_WEIGHTS_FILE, config.PRECISE_INPUT_SIZE,
config.PRECISE_CONFIDENCE_THRESHOLD, config.PRECISE_NMS_THRESHOLD,
config.PRECISE_DNN_BACKEND, config.PRECISE_DNN_TARGET)
print("Loading smoothie...")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
print("Loading vesc...")
vesc_engine = adapters.VescAdapter(config.VESC_RPM_SLOW,
config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
print("Loading gps...")
gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
# gps = stubs.GPSStub(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP)
print("Loading camera...")
camera = adapters.CameraAdapterIMX219_170(
config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM,
config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM,
config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM,
config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_W, config.CAMERA_H,
config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD)
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger_full.write(msg + "\n")
# ask permission to start moving
msg = "Initializing done. Press enter to start moving."
input(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ|Raw ang|Res ang|Ord ang|Sum ang|Distance |Adapter|Smoothie|'
print(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ,Raw ang,Res ang,Ord ang,Sum ang,Distance,Adapter,Smoothie,'
for field_name in report_field_names:
msg += field_name + ","
msg = msg[:-1]
logger_table.write(msg + "\n")
# path points visiting loop
msg = "Generated " + str(len(path_points)) + " points."
logger_full.write(msg + "\n")
for i in range(1, len(path_points)):
from_to = [path_points[i - 1], path_points[i]]
from_to_dist = nav.get_distance(from_to[0], from_to[1])
msg = "Current movement vector: " + str(
from_to) + " Vector size: " + str(from_to_dist)
# print(msg)
logger_full.write(msg + "\n\n")
move_to_point_and_extract(
from_to, gps, vesc_engine, smoothie, camera,
periphery_detector, precise_detector, client, logger_full,
logger_table, report_field_names, used_points_history,
config.UNDISTORTED_ZONE_RADIUS, working_zone_polygon,
working_zone_points_cv, view_zone_polygon, view_zone_points_cv,
config.DEBUG_IMAGES_PATH, nav)
msg = "Work is done."
print(msg)
logger_full.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)\n" + traceback.format_exc(
)
print(msg)
logger_full.write(msg + "\n")
except:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger_full.write(msg + "\n")
finally:
# try emergency stop
try:
vesc_engine.stop_moving()
except:
pass
print("Releasing camera...")
camera.release()
# save log data
print("Saving positions histories...")
if len(used_points_history) > 0:
save_gps_coordinates(
used_points_history,
"used_gps_history " + get_current_time() + ".txt"
) # TODO: don't accumulate a lot of points - write each of them to file as soon as they come
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
adapter_points_history = gps.get_last_positions_list(
) # TODO: reduce history positions to 1 to save RAM
if len(adapter_points_history) > 0:
save_gps_coordinates(
adapter_points_history,
"adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
# close log and hardware connections
print("Closing loggers...")
logger_full.close()
logger_table.close()
print("Disconnecting hardware...")
smoothie.disconnect()
vesc_engine.disconnect()
gps.disconnect()
# close transmitting connections
print("Closing transmitters...")
sensor_processor.endSession()
client.closeConnection()
sensor_processor.stopServer()
print("Safe disable is done.")
def main():
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
with adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc:
name = datetime.datetime.now().strftime("%d-%m-%Y %H:%M")
with utility.Logger(name + '.txt') as logger:
msg = 'Starting durability test...\n'
logger.write(msg)
print(msg)
msg = 'RPM: {0}'.format(config.VESC_RPM_SLOW)
logger.write(msg + "\n")
print(msg)
for i in range(COUNT_OF_ITERATION):
msg = 'Iteration {0}...'.format(i)
logger.write(msg + "\n")
print(msg)
try:
A_rand = rnd.randint(config.A_MIN, config.A_MAX) # 1
msg = 'Start moving the steering wheels to {0}...'.format(A_rand)
logger.write(msg + "\n")
print(msg)
response = smoothie.custom_move_to(config.A_F_MAX, A=A_rand)
# if response != smoothie.RESPONSE_OK:
logger.write(response + '\n')
print(response)
smoothie.wait_for_all_actions_done()
moving_time_rand = rnd.uniform(MOVE_FORWARD_FROM, MOVE_FORWARD_TO) # 2
msg = 'Start moving the robot forward, VESC_MOVING_TIME {0}...'.format(moving_time_rand)
logger.write(msg + '\n')
print(msg)
vesc.set_moving_time(moving_time_rand)
vesc.start_moving()
vesc.wait_for_stop()
msg = 'Stop moving the robot forward, VESC_MOVING_TIME {0}...'.format(moving_time_rand)
logger.write(msg + '\n')
print(msg)
for j in range(COUNT_OF_EXTRACTION):
msg = 'Go to the extraction position Y min...'
logger.write(msg + '\n')
print(msg)
response = smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM,
Y=config.Y_MIN)
# if response != smoothie.RESPONSE_OK:
logger.write(response + '\n')
print(response)
smoothie.wait_for_all_actions_done()
point_rand = rnd.choice(config.IMAGE_CONTROL_POINTS_MAP) # 3
msg = 'Start the corkscrew movement to the control point {0}...'.format(point_rand)
logger.write(msg + '\n')
print(msg)
response = smoothie.custom_move_for(config.XY_F_MAX, X=point_rand[2] / config.XY_COEFFICIENT_TO_MM,
Y=point_rand[3] / config.XY_COEFFICIENT_TO_MM)
# if response != smoothie.RESPONSE_OK:
logger.write(response + '\n')
print(response)
smoothie.wait_for_all_actions_done()
cur_coord = smoothie.get_adapter_current_coordinates()
rand_sm_x = rnd.randint(SHIFT_MIN, SHIFT_MAX) # 4
rand_sm_y = rnd.randint(SHIFT_MIN, SHIFT_MAX)
error_msg_x = smoothie.validate_value(cur_coord['X'] * config.XY_COEFFICIENT_TO_MM,
rand_sm_x * config.XY_COEFFICIENT_TO_MM,
"X", config.X_MIN, config.X_MAX, "X_MIN", "X_MAX")
error_msg_y = smoothie.validate_value(cur_coord['Y'] * config.XY_COEFFICIENT_TO_MM,
rand_sm_y * config.XY_COEFFICIENT_TO_MM,
"Y", config.Y_MIN, config.Y_MAX, "Y_MIN", "Y_MAX")
if error_msg_x:
rand_sm_x = -rand_sm_x
if error_msg_y:
rand_sm_y = -rand_sm_y
msg = 'Start pointing the corkscrew by {0} mm along the X axis, and {1} mm along the Y axis...'.format(
rand_sm_x, rand_sm_y)
logger.write(msg + '\n')
print(msg)
response = smoothie.custom_move_for(config.XY_F_MAX, X=rand_sm_x, Y=rand_sm_y)
logger.write(response + '\n')
print(response)
smoothie.wait_for_all_actions_done()
msg = 'Starting extraction movements...' # 5
logger.write(msg + '\n')
print(msg)
response = smoothie.custom_move_for(config.Z_F_EXTRACTION_DOWN, Z=config.EXTRACTION_Z)
print(response)
logger.write(response + "\n")
smoothie.wait_for_all_actions_done()
response = smoothie.ext_cork_up()
print(response)
logger.write(response + "\n")
smoothie.wait_for_all_actions_done()
except KeyboardInterrupt:
msg = "Keyboard interrupt. Exiting."
print(msg)
logger.write(msg + "\n")
exit(0)
except:
error = traceback.format_exc()
logger.write(error + "\n")
print(error)
else:
error = 'No software errors found'
logger.write(error + '\n')
print(error)
msg = 'Done.'
logger.write(msg + '\n')
print(msg)
def main():
#meters_multiplier = ask_meters_multiplier()
#distance, force = ask_speed_mode(meters_multiplier)
rpm = float(input("Set RPM: "))
moving_time = float(input("Set moving time (seconds): "))
print("Initializing detector...")
create_directories(WITH_PLANTS_OUTPUT_PATH, NO_PLANTS_OUTPUT_PATH,
DRAWN_BOXES_OUTPUT_PATH)
detector = detection.YoloOpenCVDetection()
print("Initializing smoothie...")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
# move camera to the low-center position
print("Moving camera to position...")
smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2,
Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
print("Initializing camera...")
with CameraAdapterIMX219_170(ISP_DIGITAL_GAIN_RANGE_FROM,
ISP_DIGITAL_GAIN_RANGE_TO, GAIN_RANGE_FROM,
GAIN_RANGE_TO, EXPOSURE_TIME_RANGE_FROM,
EXPOSURE_TIME_RANGE_TO, AE_LOCK) as camera:
time.sleep(2)
counter = 1
try:
with adapters.VescAdapter(rpm, moving_time, VESC_ALIVE_FREQ,
VESC_CHECK_FREQ, VESC_PORT,
VESC_BAUDRATE) as vesc_engine:
# start moving forward
vesc_engine.start_moving()
# get and save images until keyboard interrupt
print("Starting to take photos...")
while vesc_engine.is_movement_allowed():
frame = camera.get_image()
plants = detector.detect(frame)
# sort and save photo
if len(plants) > 0:
save_image(WITH_PLANTS_OUTPUT_PATH, frame, counter,
"With plants")
if SAVE_WITH_BOXES:
detection.draw_boxes(frame, plants)
save_image(DRAWN_BOXES_OUTPUT_PATH, frame, counter,
"With boxes")
else:
save_image(NO_PLANTS_OUTPUT_PATH, frame, counter,
"No plants")
counter += 1
if counter % 100 == 0:
print("Saved", counter, "photos")
except KeyboardInterrupt:
vesc_engine.stop_moving()
print("Stopped by a keyboard interrupt (Ctrl + C)")
print("Saved", counter, "photos")
exit(0)
print("Saved", counter, "photos")
print("Done!")
def main():
nav = navigation.GPSComputing()
used_points_history = []
raw_angles_history = []
logger = utility.Logger("console " + get_current_time() + ".txt")
# QGIS and sensor data transmitting
path = os.path.abspath(os.getcwd())
sensor_processor = SensorProcessing.SensorProcessing(path, 0)
sensor_processor.startServer()
client = socketForRTK.Client.Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger.write(msg + "\n")
sensor_processor.startSession()
try:
msg = "Initializing..."
print(msg)
logger.write(msg + "\n")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
vesc_engine = adapters.VescAdapter(int(config.VESC_RPM / 2),
config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger.write(msg + "\n")
msg = "Initializing done."
print(msg)
logger.write(msg + "\n")
# pick from gps
field_gps_coords = load_coordinates(
config.INPUT_GPS_FIELD_FILE) # [A, B, C, D]
if len(field_gps_coords) != 4:
msg = "Expected 4 gps points in " + config.INPUT_GPS_FIELD_FILE + ", got " + str(
len(field_gps_coords))
print(msg)
logger.write(msg)
field_gps_coords.append(field_gps_coords[0].copy(
)) # this makes robot move back to starting point (point A)
field_gps_coords.append(field_gps_coords[1].copy())
# start moving forward
msg = "Press enter to start moving"
input(msg)
logger.write(msg + "\n")
# perimeter bypass loop
for i in range(1, len(field_gps_coords) - 1):
# current vector is marked as XY where X2 and Y1 is corner maneuver points
# example: current side is AB which marked as XY, then point X2 is on AB, Y1 is on BC and
# X2 and Y1 near ^ABC angle (XYZ). ABCD are "global" points which corresponding field.txt file
# while XYZ displays currently processed points
# XY vector
current_vector = [field_gps_coords[i - 1], field_gps_coords[i]]
cur_vec_dist = nav.get_distance(current_vector[0],
current_vector[1])
msg = "Current movement vector: " + str(
current_vector) + " Vector size: " + str(cur_vec_dist)
print(msg)
logger.write(msg)
# check if moving vector is too small for maneuvers
if config.MANEUVER_START_DISTANCE >= cur_vec_dist:
msg = "No place for maneuvers; config distance before maneuver is " + \
str(config.MANEUVER_START_DISTANCE) + " current moving vector distance is " + str(cur_vec_dist)
print(msg)
logger.write(msg + "\n")
break
# compute X2 point
else:
dist_to_x2 = cur_vec_dist - config.MANEUVER_START_DISTANCE
point_x2 = nav.get_point_on_vector(current_vector[0],
current_vector[1],
dist_to_x2)
msg = "Point X2: " + str(point_x2) + "[X X2] size: " + str(
dist_to_x2)
print(msg)
logger.write(msg)
# move to X2
from_to = [current_vector[0], point_x2]
move_to_point(from_to, used_points_history, gps, vesc_engine,
smoothie, logger, client, nav, raw_angles_history)
# TO DO: BUG - we checking YZ to be less than maneuver dist but robot will be in Y1, not in Y, so it's not
# correct. Maybe config.MANEUVER_START_DISTANCE should be multiplied by 2 when checking that vector size is
# big enough
# go from X2 to Y1 point (see description in comments above)
# YZ vector
current_vector = [field_gps_coords[i], field_gps_coords[i + 1]]
cur_vec_dist = nav.get_distance(current_vector[0],
current_vector[1])
msg = "Next movement vector: " + str(
current_vector) + " Vector size: " + str(cur_vec_dist)
print(msg)
logger.write(msg)
# check if moving vector is too small for maneuvers
if config.MANEUVER_START_DISTANCE >= cur_vec_dist:
msg = "No place for maneuvers; config distance before maneuver is " + \
str(config.MANEUVER_START_DISTANCE) + " next moving vector distance is " + str(cur_vec_dist)
print(msg)
logger.write(msg + "\n")
break
# compute Y1 point to move (current vector is marked as XY where X2 and Y1 is corner maneuver points)
# example: next side is BC which marked as XY, then point Y1 is on BC (YZ)
else:
dist_to_y1 = config.MANEUVER_START_DISTANCE
point_y1 = nav.get_point_on_vector(current_vector[0],
current_vector[1],
dist_to_y1)
msg = "Point Y1: " + str(point_y1) + "[Y Y1] size: " + str(
dist_to_y1)
print(msg)
logger.write(msg)
# move to Y1
from_to = [current_vector[0], point_y1]
move_to_point(from_to, used_points_history, gps, vesc_engine,
smoothie, logger, client, nav, raw_angles_history)
msg = "Done!"
print(msg)
logger.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)"
print(msg)
logger.write(msg + "\n")
except Exception:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger.write(msg)
finally:
# save log data
if len(used_points_history) > 0:
save_gps_coordinates(
used_points_history,
"used_gps_history " + get_current_time() + ".txt")
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger.write(msg)
adapter_points_history = gps.get_last_positions_list()
if len(adapter_points_history) > 0:
save_gps_coordinates(
adapter_points_history,
"adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger.write(msg)
# close log and hardware connections
logger.close()
smoothie.disconnect()
vesc_engine.disconnect()
gps.disconnect()
# close transmitting connections
sensor_processor.endSession()
client.closeConnection()
sensor_processor.stopServer()
def main():
create_directories(config.DEBUG_IMAGES_PATH)
working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS)
working_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
view_zone_polygon = Polygon(config.VIEW_ZONE_POLY_POINTS)
view_zone_points_cv = np.array(config.VIEW_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
nav = navigation.GPSComputing()
used_points_history = []
raw_angles_history = []
logger_full = utility.Logger("full log " + get_current_time() + ".txt")
logger_table = utility.Logger("table log " + get_current_time() + ".csv")
# sensors picking
report_field_names = [
'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current',
'avg_input_current', 'rpm', 'input_voltage'
]
report_file = open('report.csv', 'w', newline='')
report_writer = csv.DictWriter(report_file, fieldnames=report_field_names)
report_writer.writeheader()
# QGIS and sensor data transmitting
path = os.path.abspath(os.getcwd())
sensor_processor = SensorProcessing.SensorProcessing(path, 0)
sensor_processor.startServer()
client = socketForRTK.Client.Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger_full.write(msg + "\n")
sensor_processor.startSession()
try:
msg = "Initializing..."
print(msg)
logger_full.write(msg + "\n")
print("Loading periphery detector...")
periphery_detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD,
config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND,
config.PERIPHERY_DNN_TARGET)
print("Loading smoothie...")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
print("Loading vesc...")
vesc_engine = adapters.VescAdapter(config.VESC_RPM_SLOW,
config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
print("Loading GPS...")
gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
print("Loading camera...")
camera = adapters.CameraAdapterIMX219_170(
config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM,
config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM,
config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM,
config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_W, config.CAMERA_H,
config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD)
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger_full.write(msg + "\n")
# load field coordinates
field_gps_coords = load_coordinates(
config.INPUT_GPS_FIELD_FILE) # [A, B, C, D]
if len(field_gps_coords) != 4:
msg = "Expected 4 gps points in " + config.INPUT_GPS_FIELD_FILE + ", got " + str(
len(field_gps_coords))
print(msg)
logger_full.write(msg + "\n")
exit(1)
# generate path points
path_points = build_path(field_gps_coords, nav, logger_full)
if len(path_points) > 0:
save_gps_coordinates(
path_points, "generated_path " + get_current_time() + ".txt")
else:
msg = "List of path points is empty, saving canceled."
print(msg)
logger_full.write(msg + "\n")
if len(path_points) < 2:
msg = "Expected at least 2 points in path, got " + str(len(path_points)) + \
" instead (1st point is starting point)."
print(msg)
logger_full.write(msg + "\n")
exit(1)
# ask permission to start moving
msg = "Initializing done. Press enter to start moving."
input(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ|Raw ang|Res ang|Ord ang|Sum ang|Distance |Adapter|Smoothie|'
print(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ,Raw ang,Res ang,Ord ang,Sum ang,Distance,Adapter,Smoothie,'
for field_name in report_field_names:
msg += field_name + ","
msg = msg[:-1]
logger_table.write(msg + "\n")
# path points visiting loop
for i in range(1, len(path_points)):
from_to = [path_points[i - 1], path_points[i]]
from_to_dist = nav.get_distance(from_to[0], from_to[1])
msg = "Current movement vector: " + str(
from_to) + " Vector size: " + str(from_to_dist)
# print(msg)
logger_full.write(msg + "\n")
move_to_point(from_to, used_points_history, gps, vesc_engine,
smoothie, logger_full, client, nav,
raw_angles_history, report_writer,
report_field_names, logger_table, periphery_detector,
camera, working_zone_points_cv)
msg = "Done!"
print(msg)
logger_full.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)"
print(msg)
logger_full.write(msg + "\n")
except Exception:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger_full.write(msg + "\n")
finally:
try:
print("Stopping movement...")
vesc_engine.stop_moving()
except:
pass
try:
print("Releasing camera...")
camera.release()
except:
pass
# save log data
if len(used_points_history) > 0:
save_gps_coordinates(
used_points_history,
"used_gps_history " + get_current_time() + ".txt")
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
adapter_points_history = gps.get_last_positions_list()
if len(adapter_points_history) > 0:
save_gps_coordinates(
adapter_points_history,
"adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
# close log and hardware connections
logger_full.close()
logger_table.close()
report_file.close()
smoothie.disconnect()
vesc_engine.disconnect()
gps.disconnect()
# close transmitting connections
sensor_processor.endSession()
client.closeConnection()
sensor_processor.stopServer()
def main():
used_points_history = []
adapter_points_history = []
raw_angles_history = []
logger = utility.Logger("console " + get_current_time() + ".txt")
# """
path = os.path.abspath(os.getcwd())
sP = SensorProcessing(path, 0)
sP.startServer()
client = Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger.write(msg + "\n")
sP.startSession()
# """
try:
nav = navigation.GPSComputing()
msg = "Initializing..."
print(msg)
logger.write(msg + "\n")
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
with adapters.VescAdapter(config.VESC_RPM, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc_engine:
with adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP) as gps:
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger.write(msg + "\n")
msg = "Initializing done."
print(msg)
logger.write(msg + "\n")
# get route (field) and save it
msg = "Enter 1 to create and save field.txt points file, 2 to load existing file: "
command = input(msg)
msg += command
logger.write(msg + "\n")
# pick from gps
if command == "1":
msg = "Press enter to save point B"
input(msg)
logger.write(msg)
point_b = gps.get_fresh_position()
msg = "Point B saved."
print(msg)
logger.write(msg)
# ask info for moving to next point and move there
rpm = 11000
msg = "RPM = " + str(rpm) + "; set moving time (seconds; will start moving immediately): "
moving_time = input(msg)
msg += moving_time
moving_time = float(moving_time)
vesc_engine.set_rpm(11000)
vesc_engine.set_moving_time(moving_time)
vesc_engine.start_moving()
vesc_engine.wait_for_stop()
vesc_engine.set_moving_time(config.VESC_MOVING_TIME)
vesc_engine.set_rpm(config.VESC_RPM)
msg = "Press enter to save point A"
input(msg)
logger.write(msg)
point_a = gps.get_fresh_position()
msg = "Point A saved."
print(msg)
logger.write(msg)
field_gps_coords = [point_a, point_b]
save_gps_coordinates(field_gps_coords, "field " + get_current_time() + ".txt")
# load from file
elif command == "2":
field_gps_coords = load_coordinates(config.INPUT_GPS_FIELD_FILE)
else:
msg = "Wrong command, exiting."
print(msg)
logger.write(msg + "\n")
exit(1)
# start moving forward
msg = "Press enter to start moving"
input(msg)
logger.write(msg + "\n")
prev_maneuver_time = time.time()
prev_point = gps.get_fresh_position()
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.copy()[0], cur_pos.copy()[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
# check if arrived
# TO DO: it won't work if deviation > course destination diff, as robot will be far away on some
# side and will never get too close to the path ending point
distance = nav.get_distance(cur_pos, field_gps_coords[1])
# reduce speed if near the target point
if USE_SPEED_LIMIT:
if distance <= DECREASE_SPEED_TRESHOLD:
vesc_engine.apply_rpm(int(config.VESC_RPM / 2))
else:
vesc_engine.apply_rpm(config.VESC_RPM)
msg = "Distance to B: " + str(distance)
print(msg)
logger.write(msg + "\n")
if distance <= config.COURSE_DESTINATION_DIFF:
vesc_engine.stop_moving()
msg = "Arrived (allowed destination distance difference " + \
str(config.COURSE_DESTINATION_DIFF) + " mm)"
print(msg)
logger.write(msg + "\n")
break
# 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(field_gps_coords[0]) \
+ " B: " + str(field_gps_coords[1])
print(msg)
logger.write(msg + "\n")
raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos, field_gps_coords[1])
if len(raw_angles_history) >= config.WINDOW:
raw_angles_history.pop(0)
raw_angles_history.append(raw_angle)
# angle_kp = raw_angle * config.KP
sum_angles = sum(raw_angles_history)
angle_kp = raw_angle * config.KP + sum_angles * config.KI
target_angle_sm = angle_kp * -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: " + \
str(angle_kp) + " 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")
adapter_points_history = gps.get_last_positions_list()
msg = "Done!"
print(msg)
logger.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)"
print(msg)
logger.write(msg + "\n")
except Exception:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger.write(msg)
finally:
if len(used_points_history) > 0:
save_gps_coordinates(used_points_history, "used_gps_history " + get_current_time() + ".txt")
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger.write(msg)
if len(adapter_points_history) > 0:
save_gps_coordinates(adapter_points_history, "adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger.write(msg)
logger.close()
# """
sP.endSession()
client.closeConnection()
sP.stopServer()
