def fast_worker(running, robot, positions, ser, close_function):
"""
Fastworker logic
A while-loop with the main control logic and should be used for fast processes.
"""
print("Starting fastWorker in a separate thread")
# Distance from the START marker to the wall in mm
start_to_wall_dist = 1800
last_ls1 = 0
# markers_count = 0
on_marker = False
robot.reset_encoders()
marker_state = False
while running:
arduino_data = sensors.get_data_from_arduino(ser)
"""
TASK: Get the averaged encoder value and use it to
find the distance from the wall in millimetres
positions['current_enc'] = ...
"""
if arduino_data:
ls1 = arduino_data['ls1']
ls2 = arduino_data['ls2']
ls3 = arduino_data['ls3']
ls4 = arduino_data['ls4']
ls5 = arduino_data['ls5']
us_pos = arduino_data['us']
"""
TASK: save current ultrasonic position to positions dictionary
"""
encoder_value = robot.read_encoders_average()
# print('encoder value ---> ', encoder_value)
positions['current_us'] = us_pos
positions['current_marker'], on_marker = line.markers_detected(
ls1, positions['current_marker'], last_ls1)
fusion.on_ultrasonic_measurement(us_pos)
"""
Add the rest of your line following & marker detection logic
"""
# positions['current_marker'] = line.markers_detected(ls1, last_ls1,us_pos, on_marker)
# print('# ---------------- remaining_dist to wall -------------> ',positions['current_marker'])
if positions['current_marker'] == 1 and marker_state == False:
robot.reset_encoders()
marker_state = True
start_to_wall_dist = 1800
elif positions['current_marker'] == 2 and marker_state == True:
robot.reset_encoders()
start_to_wall_dist = 1600
marker_state = False
elif positions['current_marker'] == 3 and marker_state == False:
robot.reset_encoders()
start_to_wall_dist = 1380
marker_state = True
elif positions['current_marker'] == 4 and marker_state == True:
robot.reset_encoders()
start_to_wall_dist = 1100
marker_state = False
elif positions['current_marker'] == 5 and marker_state == False:
robot.reset_encoders()
start_to_wall_dist = 700
marker_state = True
elif positions['current_marker'] == 6 and marker_state == True:
robot.reset_encoders()
start_to_wall_dist = 400
marker_state = False
elif positions['current_marker'] == 7:
start_to_wall_dist = 200
robot.stop()
else:
line.follow(robot, ls1, ls2, ls3, ls4, ls5, us_pos)
remaining_dist = start_to_wall_dist - encoder_value * 10
positions['current_enc'] = remaining_dist
fusion.on_encoder_measurement(positions['current_enc'])
if not ser.is_open:
close_function("Serial is closed!")
# Limit control thread to 50 Hz
time.sleep(0.02)
close_function("Fast_worker closed!")
if __name__ == "__main__":
# Register a callback for CTRL+C
signal.signal(signal.SIGINT, signal_handler)
robot = go.EasyGoPiGo3()
robot.set_speed(100)
running, ser = sensors.initialize_serial('/dev/ttyUSB1')
# arduino_data = get_data_from_arduino(ser)
markers_count = 0 # Change to correct value
on_marker = False
while running:
arduino_data = sensors.get_data_from_arduino(ser)
# arduino_data = get_data_from_arduino(ser)
if arduino_data:
# Extract the sensor values
ls1 = arduino_data['ls1']
ls2 = arduino_data['ls2']
ls3 = arduino_data['ls3']
ls4 = arduino_data['ls4']
ls5 = arduino_data['ls5']
us = arduino_data['us']
# Print received to the console
print("LS1: ", ls1, "LS2: ", ls2, "LS3: ", ls3, "LS4: ", ls4,
"LS5: ", ls5, "US: ", us)