mazePERF

"""MAZE 1."""
import rospy
from PiBot import PiBot


class Robot:
    """
    The three primitives robotic paradigm: sense, plan, act [1].

    Sense - gather information using the sensors
    Plan - create a world model using all the information and plan the next move
    Act - carry out the next step of the plan using actuators
    [1] https://en.wikipedia.org/wiki/Robotic_paradigm
    """

    def __init__(self):
        """Begin robot."""
        print("Initializing...")
        self.robot = PiBot()
        self.start_timestamp = rospy.get_time()
        self.problem_solved = False
        self.robot.set_grabber_height(100)
        self.robot.close_grabber(100)
        if self.robot.is_simulation():
            self.speed = 15
        else:
            self.speed = 12
        self.no_speed = 0

    def move_straight(self):
        """Moving forward."""
        self.robot.set_left_wheel_speed(-self.speed)
        self.robot.set_right_wheel_speed(-self.speed)

    def move_backwards(self):
        """Moving backwards."""
        self.robot.set_left_wheel_speed(self.speed)
        self.robot.set_right_wheel_speed(self.speed)

    def stop(self):
        """Stop function."""
        self.robot.set_left_wheel_speed(self.no_speed)
        self.robot.set_right_wheel_speed(self.no_speed)

    def turn_left(self):
        """Turn left function."""
        self.robot.set_left_wheel_speed(-self.speed)
        self.robot.set_right_wheel_speed(self.speed)

    def turn_right(self):
        """Turn right function."""
        self.robot.set_left_wheel_speed(self.speed)
        self.robot.set_right_wheel_speed(-self.speed)

    def drive_to_wall(self):
        """Drive hard."""
        left_side = self.robot.get_rear_left_side_ir()
        left_straight = self.robot.get_rear_left_straight_ir()
        right_straight = self.robot.get_rear_right_straight_ir()
        print("driving to wall and turning left")
        while True:
            # sõidab seinani ja pöörab vasakule
            left_straight = self.robot.get_rear_left_straight_ir()
            right_straight = self.robot.get_rear_right_straight_ir()
            left_side = self.robot.get_rear_right_side_ir()
            # driving till sees a wall and turns (90degrees)
            if left_straight < 0.045 or right_straight < 0.045:
                self.stop()
                rospy.sleep(2)
                break
            elif left_side < 0.05:
                self.stop()
                self.wall_follower()
                break
            else:
                self.move_straight()

    def wall_follower(self):
        """Follow wall."""
        left_side = self.robot.get_rear_left_side_ir()
        print("wall follower")
        while True:
            left_side = self.robot.get_rear_left_side_ir()
            left_straight = self.robot.get_rear_left_straight_ir()
            right_straight = self.robot.get_rear_right_straight_ir()
            left_diagonal = self.robot.get_rear_left_diagonal_ir()
            print(left_side)
            # peatub kui näeb seina
            if left_straight < 0.06 and right_straight < 0.06:
                self.stop()
                break
            # kui sein on liiga kaugel, pöörab sitaks seina poole
            elif left_side >= 0.07:
                self.robot.set_left_wheel_speed(-8)
                self.robot.set_right_wheel_speed(-13)
            # kui sein on kaugel, pöörab seina poole
            elif left_side >= 0.04:
                self.robot.set_left_wheel_speed(-11)
                self.robot.set_right_wheel_speed(-14)
            # diagonal panic
            elif left_diagonal < 0.03:
                print("LEFT DIAGONAL ON")
                print(left_diagonal)
                self.turn_left()
            # kui sein liiga lähedal, siis pöörab sealt eemale
            elif left_side <= 0.02:
                self.robot.set_left_wheel_speed(-12)
                self.robot.set_right_wheel_speed(-6)
            elif left_side <= 0.03:
                self.robot.set_left_wheel_speed(-14)
                self.robot.set_right_wheel_speed(-11)
            else:
                self.robot.set_left_wheel_speed(-15)
                self.robot.set_right_wheel_speed(-14)

    def corner_move(self):
        """When robot get into corner. Move A."""
        print("doing corner move")
        right_encoder = self.robot.get_right_wheel_encoder()
        encoder = right_encoder
        # turns right 90 degrees
        while right_encoder >= encoder - 500:
            right_encoder = self.robot.get_right_wheel_encoder()
            self.turn_right()
        # drive 1 sec backwards
        self.move_backwards()
        rospy.sleep(1)
        right_encoder = self.robot.get_right_wheel_encoder()
        encoder = right_encoder
        # turns left 180 degrees
        while right_encoder <= encoder + 1000:
            right_encoder = self.robot.get_right_wheel_encoder()
            self.turn_left()
        self.stop()

    def corner_move2(self):
        """When robot get into corner. Move A."""
        print("doing corner move 2")
        right_encoder = self.robot.get_right_wheel_encoder()
        encoder = right_encoder
        # turns left 90 degrees
        while right_encoder <= encoder + 500:
            right_encoder = self.robot.get_right_wheel_encoder()
            self.turn_left()
        self.stop()


if __name__ == "__main__":
    robot = Robot()
    if robot.robot.is_simulation():
        robot.drive_to_wall()
        while True:
            robot.corner_move()
            robot.wall_follower()
    else:
        while True:
            robot.wall_follower()
            robot.corner_move2()