Example #1
0
def listen():
    key = event.char
    if (key.lower() == "w"):
        robot.forward()
    else:
        gpio.cleanup()
    time.sleep(0.025)
Example #2
0
def control_robot():
    global ts, old_id
    ts = time.time()
    direction = request.args.get('direction', 0)
    mousedown = request.args.get('mousedown', 0)
    user_age = int(request.args.get('user_age', 0))
    user_id = request.args.get('user_id', 0)
    for robot in ROBOTS:
        if user_id == robot.user_id:
            if mousedown == '1':
                print('button pressed:', direction)
                if direction == 'up':
                    robot.forward()
                elif direction == 'left':
                    robot.left()
                elif direction == 'right':
                    robot.right()
                else:
                    robot.reverse()
            else:
                robot.stop()
            return jsonify(color=robot.color, endgame=0)
        elif user_id == old_id:
            return jsonify(color=robot.color, endgame=1)
    # if above didn't return, new user:
    oldest_robot = max(ROBOTS, key=lambda x: x.age())
    old_id = oldest_robot.user_id
    oldest_robot.user_id = user_id
    oldest_robot.user_age = user_age
    oldest_robot.user_time = time.time()
    print("new user: {}, {}, assigned {}".format(user_id, user_age,
                                                 robot.color))
    return jsonify(color=robot.color, endgame=0)
Example #3
0
async def handle_command(websocket, path):
    while True:
        message = await websocket.recv()

        message = literal_eval(message)
        print(message)

        if message['command'] == 'moveForward':
            robot.forward()
            answer = {'action': 'moveForward'}
        elif message['command'] == 'moveBackward':
            robot.backward()
            answer = {'action': 'moveBackward'}
        elif message['command'] == 'turnRight':
            robot.right()
            answer = {'action': 'turnRight'}
        elif message['command'] == 'turnLeft':
            robot.left()
            answer = {'action': 'turnLeft'}
        elif message['command'] == 'stop':
            robot.stop()
            answer = {'action': 'stop'}
        elif message['command'] == 'grabberGrab':
            # grabber action grab
            answer = {'action': 'grabberGrab', 'value': message['value']}
        elif message['command'] == 'grabberTilt':
            # grabber action tilt
            answer = {'action': 'grabberTilt', 'value': message['value']}
        elif message['command'] == 'grabberHeight':
            # grabber action height
            answer = {'action': 'grabberHeight', 'value': message['value']}
        await websocket.send(json.dumps(answer))
Example #4
0
def execute(img):
    global width
    global height
    global isObject
    global isMoving
    global start
    
    image = cv2.resize(img, (300,300))
        
    # compute all detected objects
    detections = model(image)
    
    # draw all detections on image
    for det in detections[0]:
        bbox = det['bbox']
        text = str(category_map[det['label']])
        cv2.rectangle(image, (int(width * bbox[0]), int(height * bbox[1])), (int(width * bbox[2]), int(height * bbox[3])), (255, 0, 0), 2)
        cv2.putText(image, text, (int(width * bbox[0]), int(height * bbox[3])), cv2.FONT_HERSHEY_PLAIN, 1.0, (0, 0, 255), lineType=cv2.LINE_AA) 
    
    # select detections that match selected class label
    matching_detections = [d for d in detections[0] if d['label'] == 53]
    
    # get detection closest to center of field of view and draw it
    det = closest_detection(matching_detections)
    if det is not None:
        bbox = det['bbox']
        cv2.rectangle(image, (int(width * bbox[0]), int(height * bbox[1])), (int(width * bbox[2]), int(height * bbox[3])), (0, 255, 0), 5)

    if det is None:
        if isObject == True:
            start = time.time()
            isObject = False
        if (time.time() - start) > 4:
            robot.stop()
        
    # otherwsie steer towards target
    else:
        if isMoving == False:
            start = time.time()
            isMoving = True
            center = detection_center(det)
            print(center)
            # move robot forward and steer proportional target's x-distance from center
            if center[0] > 0.2:
                print('left')
                robot.left()
            elif center[0] < -0.2:
                print('right')
                robot.right()
            elif center[0] > -0.2 and center[0] < 0.2:
                print('forward')
                robot.forward() 
        if (time.time() - start) > 2:
            isMoving = False

    return cv2.imencode('.jpg', image)[1].tobytes()
Example #5
0
    def do_POST(self):

        global f
        global b
        global r
        global l
        global robot

        self._set_headers()
        form = cgi.FieldStorage(fp=self.rfile,
                                headers=self.headers,
                                environ={'REQUEST_METHOD': 'POST'})

        print form.getvalue("forward")
        print form.getvalue("backward")
        print form.getvalue("left")
        print form.getvalue("right")

        if form.getvalue("forward") == "Forward":
            if f == 0:
                robot.stop()
                robot.forward()
                f = 1
            else:
                robot.stop()
                f = 0
        if form.getvalue("backward") == "Backward":
            if b == 0:
                robot.stop()
                robot.backward()
                b = 1
            else:
                robot.stop()
                b = 0
        if form.getvalue("left") == "Left":
            if l == 0:
                robot.stop()
                robot.left()
                l = 1
            else:
                robot.stop()
                l = 0
        if form.getvalue("right") == "Right":
            if r == 0:
                robot.stop()
                robot.right()
                r = 1
            else:
                robot.stop()
                r = 0

        file = open("index.html", "r")
        page = file.read()
        file.close()

        self.wfile.write(page)
Example #6
0
def move(direction):
    if direction == 'forward':
       robot.forward(0.5)
    if direction == 'backward':
       robot.backward(0.5)
    if direction == 'left':
       robot.left(0.5)
    if direction == 'right':
       robot.right(0.5)
    return '{}'
Example #7
0
def action(client, userdata, message):
    data = message.payload.decode()
    data = json.loads(data)
    data = data['action']
    print(data)
    if data == 'forward':
        print('forward')
        robot.forward()
    elif data == 'stop':
        print('stop')
        robot.stop()
    elif data == 'left':
        print('left')
        robot.left()
    elif data == 'right':
        print('right')
        robot.right()
    elif data == 'backward':
        print('backward')
        robot.backward()
    elif data == 'servo left':
        print('servo left')
        robot.servo_left()
    elif data == 'servo center':
        print('servo center')
        robot.servo_center()
    elif data == 'servo right':
        print('servo right')
        robot.servo_right()
    elif data == 'lights':
        print('lights')
        robot.lights()
    elif data == 'blinkers':
        print('blinkers')
        robot.blinkers()
    elif data == 'voltage':
        print('voltage')
        voltage = robot.voltage()
        voltage = send_json(voltage)
        myMQTT.publish('from_robot', voltage, 0)
    elif data == 'distance':
        print('distance')
        distance = robot.distance()
        distance = send_json(distance)
        myMQTT.publish('from_robot', distance, 0)
    else:
        pass
Example #8
0
def move_div():
    #if flask.request.method == 'POST':
    if flask.request.form['button'] == 'forward':
        print('forward!')
        robot.forward()
    elif flask.request.form['button'] == 'stop':
        print('stop!')
        robot.stop()
    elif flask.request.form['button'] == 'left':
        print('left')
        robot.left()
    elif flask.request.form['button'] == 'right':
        print('right')
        robot.right()
    elif flask.request.form['button'] == 'backward':
        print('backward')
        robot.backward()
    else:
        pass
    return flask.render_template('index.html')
Example #9
0
    def findLineCtrl(self, posInput, setCenter):  #2
        if not posInput:
            robot.robotCtrl.moveStart(speedMove, 'no', 'no')
            return

        if posInput > (setCenter + findLineError):
            #turnRight
            robot.right()
            self.CVCommand = 'Turning Right'
            print('Turning Right')

        elif posInput < (setCenter - findLineError):
            #turnLeft
            robot.left()
            self.CVCommand = 'Turning Left'
            print('Turning Left')

        else:
            #forward
            robot.forward()
            self.CVCommand = 'Forward'
            print('Forward')
def robot_code():
    robot.forward(20)
    robot.turn_right(90)
    robot.forward(40)
    robot.turn_left(90)
    robot.forward(40)
    robot.turn_right(90)
    robot.backwards(40)
    robot.stop()
Example #11
0
import robot

robot.pause_for(2.00) # s
for count in range(4):
    robot.forward(50.0) # cm
    robot.left(90.0) # degrees

robot.send_command_list()
Example #12
0
         [1,1,1,1,1,1,1,1,1,1]]
robot.create(world, direction, speed)

# INSTRUCTIONS
# robot.turn_on()           Power on
# robot.turn_off()          Power off
# robot.forward()           Go forward
# robot.left()              Turn left
# robot.right()             Turn right
# robot.has_finished()      Returns True if game has finished
# robot.sense()             Sensor determines if there's a wall
#
# Start game and PRESS ENTER to active the robot

# Let's go!!
robot.turn_on()

# YOUR CODE HERE
robot.forward()

# Power off the robot
robot.turn_off()








Example #13
0
def forward():
    global currentSpeed, currentCommand
    robot.forward(currentSpeed)
    carLeds.execute("forward")
Example #14
0
# robot to take to come to a stop.
#
# If you want the robot to move backwards (or it is moving
# backwards and you want it to come to a stop), just put a
# negative sign in front of every value v.
#
# Finally, if you want the robot to rotate, enter:
#
#    robot.left(theta)   # deg
#
# where theta is the number of degrees you want the robot
# to rotate in the counter-clockwise direction. Use a
# negative value if you want the robot to rotate clockwise.                                        # 
#
# Note that the Python interpreter ignores anything written
# after a sharp character (#).  In IDLE, text that the interpreter
# will ignore (like this text) is red.
#
# You can start your list of commands on the line after
# this line.  We have written one command for you:

robot.pause_for(1.00) # s
robot.forward(10.00) # cm

#############################################################
# If you want your program to work, don't change this part! #
#                                                           #
robot.send_command_list()                                   #
#                                                           #
#############################################################
import robot
'''
We import robot. This lets us call all of the functions in robot.py.

We set robot_debug to True to test our code without sending it to the robot.
Set robot_debug to False when you are ready to run your code on the robot.
'''
robot.robot_debug = False

robot.robot_speed = 200

robot.forward(5.0)
robot.forward(1.0)
robot.left(1.5)
robot.right(1.0)
robot.blink(1.0)
robot.forward(4.5)

#
# robot.robot_speed = 125
# robot.forward(3)
# robot.right_rot(.7)
# robot.forward(2.5)
# robot.left_rot(.7)
# robot.forward(5)
# robot.forward(4)
# robot.left_rot(.7)
# robot.forward(3)
#
# robot.forward(3)
# robot.right_rot(.7)
Example #16
0
def run_along_obstacle(robot, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar):

    log(INFO, 'Starting along obstacle mode')

    front_pan_tilt.reset()
    back_pan_tilt.reset ()

    # ###################################### #
    # Find direction of the closest obstacle #
    # ###################################### #

    log(INFO, 'Looking for the best direction to start')

    closest_obstacle_distance, closest_obstacle_angle = find_closest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar)

    # ##################################### #
    # Get parallel to this closest obstacle #
    # ##################################### #

    if closest_obstacle_angle < 0:

        log(INFO, 'Closest obstacle is on the right')

        is_obstacle_on_the_right = True

        if -90 < closest_obstacle_angle < 0:

            robot.left_with_angle(90 + closest_obstacle_angle, True)

        else:

            robot.right_with_angle(-closest_obstacle_angle - 90, True)

    else:

        log(INFO, 'Closest obstacle is on the left')

        is_obstacle_on_the_right = False

        if 0 < closest_obstacle_angle < 90:

            robot.right_with_angle(90 - closest_obstacle_angle, True)

        else:

            robot.left_with_angle(closest_obstacle_angle - 90, True)

    log(INFO, 'Now parallel to the closest obstacle')

    # ############################## #
    #     Now move along obstacle    #
    # ############################## #

    if is_obstacle_on_the_right == True:

        back_pan_tilt.pan_go_left()

    else:

        back_pan_tilt.pan_go_right()

    reference_distance = back_lidar.get_distance()

    log(INFO, 'Reference distance: {:06.3f}'.format(reference_distance))

    while (control.main_mode == MODE.ALONG_OBSTACLE) and (control.is_mode_aborted == False):

        robot.forward(TARGET_SPEED_STEP)

        front_distance = front_lidar.get_distance()
        side_distance  = back_lidar.get_distance()

        delta_distance = side_distance - reference_distance

        if front_distance < AVOID_OBSTACLE_STOP_DISTANCE:

            log(INFO, 'Obstacle ahead: changing direction')

            robot.stop()

            if is_obstacle_on_the_right == True:

                robot.left_with_angle(45, True)

            else:

                robot.right_with_angle(45, True)

            log(INFO, 'Ready to restart along obstacle')

        elif side_distance > 2 * reference_distance:

            log(INFO, 'Lost obstacle: changing direction')

            robot.stop()

            if is_obstacle_on_the_right == True:

                robot.right_with_angle(45, True)

            else:

                robot.left_with_angle(45, True)

            robot.forward(TARGET_SPEED_STEP)

            # Wait a bit for a move to be done
            time.sleep(1)

            log(INFO, 'Ready to restart along obstacle')

        elif -ALONG_OBSTACLE_PRECISION < delta_distance < ALONG_OBSTACLE_PRECISION:

            log(DEBUG, 'Obstacle is at the expected distance: moving on...')

            robot.stop_turn()

        elif delta_distance > 0:

            log(DEBUG, 'Going away from obstacle: correcting...')

            if is_obstacle_on_the_right == True:

                robot.right_with_strength(LEFT_RIGHT_TURN_STRENGTH)

            else:

                robot.left_with_strength(LEFT_RIGHT_TURN_STRENGTH)

        else:

            log(DEBUG, 'Going away from obstacle: correcting...')

            if is_obstacle_on_the_right == True:

                robot.left_with_strength(LEFT_RIGHT_TURN_STRENGTH)

            else:

                robot.right_with_strength(LEFT_RIGHT_TURN_STRENGTH)

    robot.stop()

    front_pan_tilt.reset()
    back_pan_tilt.reset ()
Example #17
0
import robot
'''
We import robot. This lets us call all of the functions in robot.py.

We set robot_debug to True to test our code without sending it to the robot.
Set robot_debug to False when you are ready to run your code on the robot.
'''
robot.robot_debug = False

robot.robot_speed = 150

robot.forward(1.0)
robot.forward(1.0)
robot.left(1.0)
robot.right(1.0)
robot.blink(10)
robot.forward(1.0)

#
# robot.robot_speed = 125
# robot.forward(3)
# robot.right_rot(.7)
# robot.forward(2.5)
# robot.left_rot(.7)
# robot.forward(5)
# robot.forward(4)
# robot.left_rot(.7)
# robot.forward(3)
#
# robot.forward(3)
# robot.right_rot(.7)
Example #18
0
                obv = int(obv)
                oba = oba - 100
                obv = obv - 100
            except:
                pass

            #oba=60
            print("obstacle at: {}".format(oba))
            print("visible at: {}".format(obv))

            #thoorama irruku'
            if ((x <= 400) and (x >= 200)):
                #desti is straight but check for foreign objs
                if ((oba < 50) or (oba > 150)):
                    #go straight
                    robot.forward(0.5)

                elif (x > 290):
                    robot.left(0.55)
                    robot.forward(2.5)
                    robot.right(0.55)
                elif (x <= 290):
                    robot.right(0.55)
                    robot.forward(2.5)
                    robot.left(0.55)
            elif (x > 400):
                robot.right(0.1)
            elif (x < 200):
                robot.left(0.1)
        elif (h > 100):
            robot.stopp()
Example #19
0
def forward():
	global currentSpeed, currentCommand
	robot.forward(currentSpeed)
	carLeds.execute("forward")
Example #20
0
            print('Connection Refused')
            time.sleep(5)
        else:
            print('Connected')
            state = True
            break

    print('Waiting for data...')
    data = s.recv(2048)
    if not data:
        s.close()
        state = False
    print(data.decode())
    if data == 'forward':
        print('forward!')
        robot.forward()
    elif data == 'stop':
        print('stop!')
        robot.stop()
    elif data == 'left':
        print('left')
        robot.left()
    elif data == 'right':
        print('right')
        robot.right()
    elif data == 'backward':
        print('backward')
        robot.backward()
    elif data == 'servo left':
        print('servo left')
        robot.servo_left()
Example #21
0
def console_thread(robot, left_motor, right_motor, imu_device, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar, speed_pid_controller):

    print_help()

    is_console_on = True
    selected_pid  = 1

    while is_console_on == True:

        print('> ', end = '')

        user_input = input()

        if len(user_input) == 1:

            command = user_input[0]

            if command == '1':
                print('')
                print('Speed PID selected')
                print('')
                selected_pid = 1
            elif command == 'm':
                print('')
                left_motor.print_info ('LEFT  DC MOTOR')
                right_motor.print_info('RIGHT DC MOTOR')
                print('')
            elif command == 'v':
                print('')
                front_pan_tilt.print_info('FRONT')
                print('')
                back_pan_tilt.print_info ('BACK' )
                print('')
            elif command == 'c':
                print('')
                print('SPEED PID:')
                speed_pid_controller.print_info()
                print('')
            elif command == 'u':
                print('')
                imu_device.print_info()
                print('')
            elif command == 'r':
                print('')
                robot.print_info()
                print('')
                front_lidar.print_info('FRONT LIDAR')
                back_lidar.print_info ('BACK  LIDAR')
                print('')
                print('Line following threshold: {}'.format(control.line_threshold))
                print('')
            elif command == 'q':
                print('')
                print('***** GOING TO OPERATIONAL MODE *****')
                is_console_on = False
            elif command == 'x':
                print('***** EXITING GRACEFULLY *****')
                robot.stop()
                front_pan_tilt.stop()
                back_pan_tilt.stop ()
                if status.is_rpi_gpio_used:
                    RPi.GPIO.cleanup()
                os._exit(0)
            elif command == 'h':
                print_help()

        elif len(user_input) == 2:

            command = user_input[0:2]

            if command == 'sf':
                robot.forward_step()
            elif command == 'sb':
                robot.backward_step()
            elif command == 'sl':
                robot.left_step()
            elif command == 'sr':
                robot.right_step()

        elif len(user_input) > 2 and user_input[1] == '=':

            command = user_input[0]
            value   = float(user_input[2:])

            if command == 'l':
                if value == 0:
                    log_set_level(NO_LOG )
                elif value == 1:
                    log_set_level(ERROR  )
                elif value == 2:
                    log_set_level(WARNING)
                elif value == 3:
                    log_set_level(INFO   )
                elif value == 4:
                    log_set_level(DEBUG  )
                else:
                    print('Invalid log level')
            elif command == 'p':
                if selected_pid == 1:
                    speed_pid_controller.set_kp(value)
            elif command == 'i':
                if selected_pid == 1:
                    speed_pid_controller.set_ki(value)
            elif command == 'd':
                if selected_pid == 1:
                    speed_pid_controller.set_kd(value)
            elif command == 'w':
                if selected_pid == 1:
                    speed_pid_controller.set_anti_wind_up(value)
            elif command == 's':
                if value > 0:
                    robot.forward(value)
                elif value < 0:
                    robot.backward(-value)
                else:
                    robot.stop()
            elif command == 'a':
                if value < 0:
                    robot.right_with_angle(-value, True)
                else:
                    robot.left_with_angle(value, True)
            elif command == 'g':
                if value < 0:
                    robot.right_with_strength(-value)
                else:
                    robot.left_with_strength(value)
            elif command == 't':
                control.line_threshold = value

        elif len(user_input) > 2 and user_input[2] == '=':

            command = user_input[0:2]
            value   = float(user_input[3:])

            if command == 'fp':
                front_pan_tilt.set_pan(value)
            elif command == 'ft':
                front_pan_tilt.set_tilt(value)
            elif command == 'bp':
                back_pan_tilt.set_pan(value)
            elif command == 'bt':
                back_pan_tilt.set_tilt(value)
Example #22
0
import robot
print "forward" 
robot.forward(5)

print "right"
robot.right(2)

print "left"
robot.left(2)

print "reverse"
robot.reverse(2)
print "stop"
Example #23
0
def run_corridor_following(robot, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar):

    log(INFO, 'Starting corridor following mode')

    front_pan_tilt.reset()
    back_pan_tilt.reset ()

    # ###################################### #
    # Find direction of the closest obstacle #
    # ###################################### #

    log(INFO, 'Looking for the best direction to start')

    closest_obstacle_distance, closest_obstacle_angle = find_closest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar)

    # ############################ #
    # Aim at this closest obstacle #
    # ############################ #

    if closest_obstacle_angle < 0:

        robot.right_with_angle(-closest_obstacle_angle, True)

    else:

        robot.left_with_angle(closest_obstacle_angle, True)

    log(INFO, 'Now aiming at the closest obstacle')

    # ############################ #
    # Get centered in the corridor #
    # ############################ #

    is_centered = False

    while is_centered == False:

        front_wall_distance  = front_lidar.get_distance()
        back_wall_distance   = back_lidar.get_distance ()

        delta_distance = front_wall_distance - back_wall_distance

        if -CORRIDOR_FOLLOWING_PRECISION < delta_distance < CORRIDOR_FOLLOWING_PRECISION:

            is_centered = True

        elif delta_distance > 0:

            robot.forward_step ()

        else:

            robot.backward_step()

    log(INFO, 'Now centered in corridor')

    # ############################## #
    # Get into the forward direction #
    # ############################## #

    robot.right_with_angle(90, True)

    log(INFO, 'Ready to move in corridor')

    # ######################################## #
    #      Now move forward in corridor and    #
    # keep the best possible centered position #
    # ######################################## #

    front_pan_tilt.pan_go_left()
    back_pan_tilt.pan_go_left ()

    while (control.main_mode == MODE.FOLLOW_CORRIDOR) and (control.is_mode_aborted == False):

        robot.forward(TARGET_SPEED_STEP)

        left_wall_distance  = front_lidar.get_distance()
        right_wall_distance = back_lidar.get_distance ()

        delta_distance = left_wall_distance - right_wall_distance

        if -CORRIDOR_FOLLOWING_PRECISION < delta_distance < CORRIDOR_FOLLOWING_PRECISION:

            log(DEBUG, 'Correctly centered in corridor: moving on...')

            robot.stop_turn()

            # Allow u-turn only when robot is centered
            if control.is_u_turn_requested == True:

                log(DEBUG, 'Performing u-turn as per requested')

                robot.stop()
                robot.right_with_angle(180, True)

                control.is_u_turn_requested = False

        elif delta_distance > 0:

            log(DEBUG, 'Going away from center: correcting to the left...')

            robot.left_with_strength(LEFT_RIGHT_TURN_STRENGTH / 2)

        else:

            log(DEBUG, 'Going away from center: correcting to the right...')

            robot.right_with_strength(LEFT_RIGHT_TURN_STRENGTH / 2)

    robot.stop()

    front_pan_tilt.reset()
    back_pan_tilt.reset ()
Example #24
0
import robot
import time

espera = 2

robot.init()
for i in range(0, 4):
    robot.forward(robot.throttle_max)
    time.sleep(espera)
    robot.right(robot.throttle_max)
    time.sleep(espera / 2)
robot.stop()
Example #25
0
import robot
'''
We import robot. This lets us call all of the functions in robot.py.

We set robot_debug to True to test our code without sending it to the robot.
Set robot_debug to False when you are ready to run your code on the robot.
'''
robot.robot_debug = False

robot.robot_speed = 150

robot.forward(1.0)
robot.forward(1.0)
robot.left(1.0)
robot.right(1.0)
robot.blink(10)
robot.forward(1.0)

#
# robot.robot_speed = 125
# robot.forward(3)
# robot.right_rot(.7)
# robot.forward(2.5)
# robot.left_rot(.7)
# robot.forward(5)
# robot.forward(4)
# robot.left_rot(.7)
# robot.forward(3)
#
# robot.forward(3)
# robot.right_rot(.7)
Example #26
0
def run_obstacles_avoidance(robot, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar):

    log(INFO, 'Starting obstacles avoidance mode')

    front_pan_tilt.reset()
    back_pan_tilt.reset ()

    # ####################################### #
    # Find direction of the farthest obstacle #
    # ####################################### #

    log(INFO, 'Looking for the best direction to start')

    farthest_obstacle_distance, farthest_obstacle_angle = find_farthest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, front_pan_tilt, front_lidar, back_pan_tilt, back_lidar)

    # ############################# #
    # Aim at this farthest obstacle #
    # ############################# #

    if farthest_obstacle_angle < 0:

        robot.right_with_angle(-farthest_obstacle_angle, True)

    else:

        robot.left_with_angle(farthest_obstacle_angle, True)

    # ############################## #
    #  Now start avoiding obstacles  #
    # ############################## #

    search_direction = False

    while (control.main_mode == MODE.AVOID_OBSTACLES) and (control.is_mode_aborted == False):

        if search_direction == True:

            robot.stop()
            robot.forward_step()

            # ####################################### #
            # Find direction of the farthest obstacle #
            # ####################################### #

            log(INFO, 'Looking for the best direction to continue')

            back_pan_tilt.reset()
            farthest_obstacle_distance, farthest_obstacle_angle = find_farthest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, front_pan_tilt, front_lidar, None, None)

            if farthest_obstacle_distance < AVOID_OBSTACLE_STOP_DISTANCE:

                front_pan_tilt.reset()
                farthest_obstacle_distance, farthest_obstacle_angle = find_farthest_obstacle(PAN_ANGLE_MIN, PAN_ANGLE_MAX, OBSTACLE_FAST_SCANNING_STEP, None, None, back_pan_tilt, back_lidar)

            # ############################# #
            # Aim at this farthest obstacle #
            # ############################# #

            if farthest_obstacle_angle < 0:

                robot.right_with_angle(-farthest_obstacle_angle, True)

            else:

                robot.left_with_angle(farthest_obstacle_angle, True)

            log(INFO, 'Aiming at the farthest obstacle')

            search_direction = False

        else:

            # ############################# #
            # Get distance of next obstacle #
            # ############################# #

            next_obstacle_distance = front_lidar.get_distance()

            # ################################ #
            #  Now move forward that direction #
            # ################################ #

            if next_obstacle_distance < AVOID_OBSTACLE_STOP_DISTANCE:

                log(INFO, 'Obstacle ahead: changing direction')

                search_direction = True

            else:

                log(DEBUG, 'Path is clear: moving on...')

                robot.forward(TARGET_SPEED_STEP / 2)

    robot.stop()

    front_pan_tilt.reset()
    back_pan_tilt.reset ()