Esempio n. 1
0
class Motob:
    """
    The motor object (motob) manifests an interface between a behavior and one or more motors (a.k.a. actuators).
    It contains (at least) the following instance variables:
    1. motors - a list of the motors whose settings will be determined by the motob.
    2. value - a holder of the most recent motor recommendation sent to the motob.
    Its primary methods are:
    1. update - receive a new motor recommendation, load it into the value slot, and operationalize it.
    2. operationalize - convert a motor recommendation into one or more motor settings, which are sent to
    the corresponding motor(s)
    """
    def __init__(self):
        self.motor = Motors()
        self.value = None

    def update(self, recommendation):
        self.value = recommendation
        self.operationalize()

    def operationalize(self):
        """
        convert motor recommendation into motor setting
        send to motor
        format value: [direction, speed, duration]
        :return:
        """
        for element in self.value:
            if element[0] == "f":
                self.motor.forward(element[1], element[2])
            elif element[0] == "b":
                self.motor.backward(element[1], element[2])
            elif element[0] == "l":
                self.motor.left(element[1], element[2])
            elif element[0] == "r":
                self.motor.right(element[1], element[2])
Esempio n. 2
0
def crashTest():
    ZumoButton().wait_for_press()

    sensor = Crash_sensor()

    motor = Motors()

    counter = 0
    f_value = sensor.calculateFront()
    ir_command = sensor.calculateSides()

    while True:

        counter += 1
        if counter >= 5:

            f_value = sensor.calculateFront()
            ir_command = sensor.calculateSides()
            counter = 0

        if ir_command == "LEFT":

            motor.left(0.5, 0.1)

        elif ir_command == "RIGHT":
            motor.right(0.5, 0.1)
        elif ir_command == "BACKWARD":
            motor.backward(0.3, 0.5)
        else:
            motor.forward(0.3, 0.01)

        if f_value == 1000:
            motor.stop()
            break
Esempio n. 3
0
class Motob():
    """Motor object that manifests an interface between a behavior and motor(s)"""
    def __init__(self):

        self.motors = Motors()
        self.value = ""  # nyeste motor anbefaling

    def update(
        self, recommendation
    ):  # må snakke med arbitrator hvilken form denne parameteren har
        """Mottar ny anbefaling til motoren, legger det i value og gjør det til en operasjon"""
        if self.value != recommendation:
            self.value = recommendation
            self.motors.stop()
        self.operationalize()

    def operationalize(self):
        """konverterer en anbefaling til en eller flere motor instillinger, som sendes til korresponderende motor(er)"""
        if self.value[0] == "F":
            self.motors.forward(0.25, 0.6)
        elif self.value[0] == "B":
            self.motors.backward(0.25, 0.6)
        elif self.value[0] == "L":
            self.motors.left(
                0.5, (int(self.value[1]) * 10 + int(self.value[2])) * 0.01)
        elif self.value[0] == "R":
            self.motors.right(
                0.5, (int(self.value[1]) * 10 + int(self.value[2])) * 0.01)
        elif self.value[0] == "T":
            self.motors.set_value([-1, 1], 0.5)
        elif self.value[0] == "S":
            self.motors.stop()
Esempio n. 4
0
class Motob:

    def __init__(self):
        self.motors = Motors()
        self.value = None

    # Updates the motors value and sends direction further
    def update_motor(self, value):
        self.value = value
        print(self.value)
        self.operationalize()

    # Takes a recommendation and makes the motors do the operation.
    def operationalize(self):
        recom = self.value
        if recom[0] == 'F':
            self.motors.forward(speed=recom[1]*0.01, dur=1)    # multiply the degrees with 0.01 to get percent for speed
        elif recom[0] == 'B':
            self.motors.backward(speed=recom[1]*0.01, dur=1)
        elif recom[0] == 'L':
            self.motors.left(speed=recom[1]*0.01, dur=1)
        elif recom[0] == 'R':
            self.motors.right(speed=recom[1]*0.01, dur=1)
        elif recom[0] == 'S':
            self.motors.stop()

    # Makes the robot rotate, vector [-1, 1], [L, R].
    def rotate(self, vector):
        self.motors.set_value(vector)

    # Optional stop method, duplicate in operationalize()
    def stop(self):
        self.motors.stop()
Esempio n. 5
0
def dancer():
    m = Motors()
    m.forward(0.2, 3)
    m.backward(0.2, 3)
    m.right(0.5, 3)
    m.left(0.5, 3)
    m.backward(0.3, 2.5)
    m.set_value([0.5, 0.1], 10)
    m.set_value([-0.5, -0.1], 10)
Esempio n. 6
0
def dancer():
    ZumoButton().wait_for_press()
    m = Motors()
    m.forward(.2, 3)
    m.forward(.3, 2)
    m.right(.5, 3)
    m.left(.5, 3)
    m.backward(.3, 2.5)
    m.set_value([.5, .1], 10)
    m.set_value([-.5, -.1], 10)
Esempio n. 7
0
def dancer():
    ZumoButton().wait_for_press()
    m = Motors()
    m.forward(0.2, 3)
    m.backward(0.2, 3)
    m.right(0.5, 3)
    m.left(0.5, 3)
    m.backward(0.3, 2.5)
    m.set_value([0.5, 0.1], 10)
    m.set_value([-0.5, -0.1], 10)
Esempio n. 8
0
def dancer():
    ZumoButton().wait_for_press()
    m = Motors()
    m.forward(.2,3)
    m.backward(.2,3)
    m.right(.5,3)
    m.left(.5,3)
    m.backward(.3,2.5)
    m.set_value([.5,.1],10)
    m.set_value([-.5,-.1],10)
Esempio n. 9
0
def explorer(dist=10):
    ZumoButton().wait_for_press()
    m = Motors(); u = Ultrasonic()
    while u.update() > dist:
        m.forward(.2,0.2)
    m.backward(.1,.5)
    m.left(.5,3)
    m.right(.5,3.5)
    sleep(2)
    while u.update() < dist*5:
        m.backward(.2,0.2)
    m.left(.75,5)
Esempio n. 10
0
def explorer(dist=10):
    ZumoButton().wait_for_press()
    m = Motors(); u = Ultrasonic()
    while u.update() > dist:
        m.forward(.2,0.2)
    m.backward(.1,.5)
    m.left(.5,3)
    m.right(.5,3.5)
    sleep(2)
    while u.update() < dist*5:
        m.backward(.2,0.2)
    m.left(.75,5)
Esempio n. 11
0
def explorer(dist=10):
    m = Motors()
    u = Ultrasonic()
    while u.update() > dist:
        m.forward(0.2, 0.2)
    m.backward(.1, 0.5)
    m.left(.5, 3)
    m.right(.5, 3.5)
    sleep(2)
    while u.update() < dist * 5:
        m.backward(.2, 0.2)
    m.left(.75, 5)
Esempio n. 12
0
def dancer():
    ZumoButton().wait_for_press()
    m = Motors()
    print("hei")
    m.forward(0.2, 3)
    m.backward(0.2, 3)
    m.right(0.5, 3)
    m.left(0.5, 3)
    m.backward(0.3, 2.5)
    m.set_value([0.5, 0.1], 10)
    m.set_value([-0.5, -0.1], 10)
    print("hei2")
Esempio n. 13
0
class Motob:
    def __init__(self):
        self.motors = Motors()
        self.values = []
        self.drive_speed = 0.15
        self.turn_speed = 0.5

    def update(self, recommendation):
        self.values.append(recommendation)
        self.operationalize()

    def operationalize(self):
        """
        L: left
        R: right
        F: forward
        B: backward
        S: stop
        """
        print(self.values)
        for rec in self.values:
            direction = rec[0]
            print(direction)
            if len(rec) == 1:
                print(direction)
                if direction == 'F':
                    self.motors.set_value([self.drive_speed, self.drive_speed])
                elif direction == 'B':
                    self.motors.set_value(
                        [-self.drive_speed, -self.drive_speed])
                elif direction == 'S':
                    self.motors.stop()

            elif len(rec) == 2:
                second_param = (rec[1])

                if direction == 'L':
                    self.motors.left(self.turn_speed,
                                     Motob.degrees_to_time(second_param))
                elif direction == 'R':
                    self.motors.right(self.turn_speed,
                                      Motob.degrees_to_time(second_param))
                elif direction == 'F':
                    self.motors.forward(self.drive_speed, second_param)
                elif direction == 'B':
                    self.motors.backward(self.drive_speed, second_param)
        self.values = []

    @staticmethod
    def degrees_to_time(degrees):
        return (degrees / 360) * 6
Esempio n. 14
0
class motob:
    """Motob klasse har en motor"""
    def __init__(self):
        """Create empty list as value"""
        self.motor = Motors()
        # ("l",45,+0.5)
        self.value_drive = ["l", 0, 0]  # a list [direction, degrees, speed]
        self.value_halt = False  # The value of the halt flag
        #self.dir_list = ['l','r','f','b']

    def update(self, mot_roc):
        """sette en ny value"""
        self.value_drive = mot_roc[0]
        self.value_halt = mot_roc[1]

    def operationalize(self, dur=0.05):
        """apply value, r: Right, l:Left, f:Forward, b:Backward"""
        #dur = 3
        turn_speed = self.value_drive[1] / 100
        if turn_speed > 1:
            turn_speed = 1
        # cond_left_right = False
        # if self.value_drive[1] == 0:
        # If we are turning 0 degrees, cond_left_right = True
        # cond_left_right = True
        #print("I am in loop")
        if self.value_drive[
                0] == 'l' and self.value_drive[1] > 0 and not self.value_halt:
            # turn left and we are turning, but why not call left?
            self.motor.set_value((-self.value_drive[2], self.value_drive[2]),
                                 turn_speed)  # 10 eller dur
            #self.motor.left(self.value_drive[2], dur)
        elif self.value_drive[
                0] == 'r' and self.value_drive[1] > 0 and not self.value_halt:
            # turn right, and we are turning, try motors.right??
            self.motor.set_value((self.value_drive[2], -self.value_drive[2]),
                                 turn_speed)  # 10 eller dur
            #self.motor.right(self.value_drive[2], dur)
        elif self.value_drive[0] == 'f' and self.value_drive[
                1] == 0 and not self.value_halt:
            # Drive forward
            self.motor.forward(abs(self.value_drive[2]), dur)
            #self.motor.set_value((self.value_drive[2], self.value_drive[2]), self.value_drive[2])
        elif self.value_drive[0] == 'b' and self.value_drive[
                1] == 0 and not self.value_halt:
            # Drive backwards
            self.motor.backward(abs(self.value_drive[2]), dur)
            #self.motor.set_value((-self.value_drive[2], -self.value_drive[2]), self.value_drive[2])
        elif self.value_halt:
            # If we are done, the haltflag is True, stop
            self.motor.stop()
Esempio n. 15
0
def test_regul():
    """
    Make a square with the robot and at the end make it turn the
    other direction  (just for fun).
    """
    DISTANCE = 50
    ANGLE = 90

    motors = Motors(5)
    motors.forward(DISTANCE)
    wait_motors(motors)
    motors.turn_left(ANGLE)
    wait_motors(motors)
    motors.forward(DISTANCE)
    wait_motors(motors)
    motors.turn_left(ANGLE)
    wait_motors(motors)
    motors.forward(DISTANCE)
    wait_motors(motors)
    motors.turn_left(ANGLE)
    wait_motors(motors)
    motors.forward(DISTANCE)
    wait_motors(motors)
    # We can't turn more than 255 degrees because
    # the data send to the Arduino is a 8bits value.
    motors.turn_right(180)
    wait_motors(motors)
    motors.turn_right(90)
    wait_motors(motors)
Esempio n. 16
0
def dancer():
    ZumoButton().wait_for_press()
    m = Motors()

    u = Ultrasonic()
    print(u.getvalue())

    m.forward(.2, 3)
    m.backward(.2, 3)
    m.right(.5, 3)
    m.left(.5, 3)
    m.backward(.3, 2.5)
    m.set_value([.5, .1], 10)
    m.set_value([-.5, -.1], 10)
Esempio n. 17
0
def followTest():
    ZumoButton().wait_for_press()
    m = Motors()
    follow = FollowLine()
    time = 0
    while (time < 100):
        values = follow.isOnLine()
        if values[0] == 0:
            m.forward(0.2, 0.2)
        elif values[1] == 0 or values[1] == 1:
            m.left(0.2, 0.3)
        else:
            m.right(0.2, 0.3)
        time += 1
Esempio n. 18
0
def explorer(dist=10):
    ZumoButton().wait_for_press()
    m = Motors()
    u = Ultrasonic()
    print(u.sensor_get_value())
    while u.sensor_get_value() > dist:
        m.forward(.2, 0.2)
        print(u.value)
    m.backward(.1, .5)
    m.left(.5,3)
    m.right(.5, 3.5)
    sleep(2)
    while u.sensor_get_value() < dist*5:
        m.backward(.2, 0.2)
    m.left(.75, 5)
Esempio n. 19
0
class Motob:
    def __init__(self):
        self.motors = Motors()

    def update(self, motor_recommendation: MotorOperation):
        print(motor_recommendation)
        if motor_recommendation == MotorOperation.FORWARDS:
            self.motors.forward()
        elif motor_recommendation == MotorOperation.BACKWARDS:
            self.motors.backward()
        elif motor_recommendation == MotorOperation.TURN_LEFT:
            self.motors.left(1, 2.5)
        elif motor_recommendation == MotorOperation.TURN_RIGHT:
            self.motors.right(1, 2.5)
        else:
            self.motors.forward(0)
Esempio n. 20
0
class Server():
    def __init__(self, host, port):
        self.host = host
        print(host)
        self.port = port
        self.motors = Motors()
        
    def start(self):
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.bind((self.host, self.port))
        self.sock.listen(5)
        return self.connection()
        
    def connection(self):
        conn, addr = self.sock.accept()
        print(addr)
        while True:
            data = conn.recv(1024)
            if data.decode("utf-8") == "w":
                print('move forward')
                self.motors.forward()
            elif data.decode("utf-8") == "s":
                print("move back")
                self.motors.back()
            elif data.decode("utf-8") == "a":
                print("move left")
                self.motors.left()
            elif data.decode("utf-8") == "d":
                print("move right")
                self.motors.right()
            elif data.decode("utf-8") == "space":
                print("stopping")
                self.motors.stop()
            elif data.decode("utf-8") == "m":
                print("server OFF")
                break
            elif data.decode("utf-8") == "q":
                print("Program will close...")
                self.sock.close()
                return False
        
        self.sock.close()
        return True
        
    def restart(self):
        self.connection()
class Motob:
    def __init__(self):
        self.value = None
        self.motors = Motors()

    def update(self, v):
        self.value = v
        self.operate()

    def operate(self):
        if self.value[0] == 'S':
            self.motors.stop()
        elif self.value[0] == 'F':
            self.motors.forward(0.5, 0.3)
        elif self.value[0] == 'B':
            self.motors.backward(0.3, 0.3)
        elif self.value[0] == 'R':
            self.motors.right(0.5, 0.3)
        elif self.value[0] == 'L':
            self.motors.left(0.5, 0.3)
Esempio n. 22
0
class Motob:
    def __init__(self):
        self.motor = Motors()

    def update(self, action, sleep_time = 0.33):
        if action == "forward":
            self.motor.forward()
            sleep(sleep_time)
        elif action == "backward":
            self.motor.backward()
            sleep(sleep_time * 6)
            self.motor.left()
            sleep(sleep_time / 10)
        elif action == "left":
            self.motor.left()
            sleep(sleep_time / 10)
        elif action == "right":
            self.motor.right()
            sleep(sleep_time / 10)
        elif action == "charge":
            self.motor.set_value((400, 400))
        elif action == "brake":
            self.motor.stop()
Esempio n. 23
0
class Motob():
    def __init__(self):
        self.motor = Motors()
        self.value = None  # Most recent motor recommendation sent to the motob

    def update(self, motor_recom):
        self.value = motor_recom
        self.operationalize()

    def stop(self):
        self.motor.stop()

    def operationalize(self):
        if self.value[0] == "left":
            self.motor.stop()
            self.motor.turn_left(self.value[1])
            self.motor.forward(0.35)
        elif self.value[0] == "right":
            self.motor.stop()
            self.motor.turn_right(self.value[1])
            self.motor.forward(0.35)
        elif self.value[0] == "drive":
            self.motor.forward(self.value[1])
Esempio n. 24
0
class Robot:
    # US sensors constants.
    US_SENSORS = [
        {
            'name': 'front_bottom',
            'trigger_limit': 8,
            'sensors': 0
        },
        {
            'name': 'left',
            'trigger_limit': 10,
            'sensors': 2
        },
        {
            'name': 'back',
            'trigger_limit': 8,
            'sensors': 4
        },
        {
            'name': 'right',
            'trigger_limit': 10,
            'sensors': 3
        },
        {
            'name': 'front_top',
            'trigger_limit': 8,
            'sensors': 1
        },
    ]

    # Take in count the obstacle dimension (assuming another robot)
    # + the robot size.
    OBSTACLES_DIMENSION = 50

    def __init__(self):
        """
        position: init position dict with keys:
            - "angle": a X-axis relative angle.
            - "point": the first position.
        """
        #  self._us_sensors = RangeSensor(4)
        self._motors = Motors(5)
        self._kinematic = Kinematics(6)
        self._us = RangeSensor(4)

        logging.info('Building the graph map.')
        #  self._graph = build_graph(robot_diagonal)
        logging.info('Finished to build the graph map.')
        self._blocking_servo = -1

    def reset_kinematics(self):
        self._kinematic.up_clamp()
        time.sleep(0.2)
        self._kinematic.open_clamp()
        time.sleep(0.2)
        self._kinematic.reset_funny()
        time.sleep(0.2)
        self._kinematic.push_back()
        time.sleep(1)

    def wait_motors(self, enable=True, timeout=0):
        first_time = time.time()
        while not self._motors.is_done():
            if enable:
                if self._blocking_servo != -1:
                    i = self._blocking_servo
                    test = self._us.get_range(i)
                    print(test)
                    if test > self.US_SENSORS[i]['trigger_limit']:
                        self._motors.restart()
                        self._blocking_servo = -1
                    else:
                        continue
                us_sensors = self._us.get_ranges()
                print(us_sensors)
                for i, us in enumerate(us_sensors):
                    if 'front' in self.US_SENSORS[i][
                            'name'] or 'back' in self.US_SENSORS[i]['name']:
                        if us < self.US_SENSORS[i]['trigger_limit']:
                            self._motors.stop()
                            self._blocking_servo = i
                            break
            if (time.time() - first_time) > timeout and timeout != 0:
                break
            time.sleep(0.1)

    def take_modules(self, number=1):
        for i in range(number):
            self._kinematic.open_clamp()
            self._kinematic.down_clamp()
            time.sleep(DELAY_UP_DOWN_CLAMP)
            self._motors.forward(9)
            self.wait_motors(enable=False, timeout=2)

            self._kinematic.close_clamp()
            time.sleep(1)
            #  self._motors.set_speed(40)
            self._motors.backward(7)
            if number == 1:
                self._motors.forward(6)
                self.wait_motors(enable=False, timeout=2)
                self._motors.backward(6)
                self.wait_motors(enable=False, timeout=2)
            self.wait_motors(enable=False)
            self._motors.backward(8)
            self.wait_motors(enable=False)

            #SEULEMENT SI ON DECIDE DE RECULER POUR MIEUX PRENDRE LE MODULE ---
            self._kinematic.open_clamp()
            time.sleep(0.5)
            self._motors.forward(3)
            self.wait_motors(enable=False, timeout=2)
            self._kinematic.close_clamp()
            time.sleep(1)
            #---

            self._motors.forward(3)
            self._kinematic.up_clamp()
            self.wait_motors(enable=False, timeout=3)
            time.sleep(0.5)
            self._kinematic.open_clamp()
            time.sleep(1)
        self._kinematic.down_clamp()
        time.sleep(0.5)
        self._kinematic.up_clamp()

    def eject_modules(self, number=1):
        for i in range(number):
            self._kinematic.push_out()
            time.sleep(1.5)
            self._kinematic.push_back()
            time.sleep(1.5)
        self._motors.forward(2)
        self.wait_motors(timeout=2)
        self._motors.backward(8)
        self.wait_motors(timeout=2)

    def get_kin(self):
        return self._kinematic

    def get_motors(self):
        return self._motors

    def finalize(self):
        self._motors.stop()

        self._kinematic.launch_funny()
        time.sleep(0.8)
        self._kinematic.reset_funny()
Esempio n. 25
0
class Robot:
    _DELAY_OPEN_ClOSE_CLAMP = 0.7
    _DELAY_UP_DOWN_CLAMP = 2.5
    _DELAY_IN_OUT_BLOCK = 3

    # US sensors constants.
    US_SENSORS = [
        {
            'name': 'front_bottom',
            'trigger_limit': 10,
            'sensors': [0, 1]
        },
        {
            'name': 'left',
            'trigger_limit': 10,
            'sensors': [2]
        },
        {
            'name': 'back',
            'trigger_limit': 5,
            'sensors': [4]
        },
        {
            'name': 'right',
            'trigger_limit': 10,
            'sensors': [3]
        },
        {
            'name': 'front_top',
            'trigger_limit': 10,
            'sensors': [0, 1]
        },
    ]

    DIMENSION = {'length': 32.5, 'width': 19.2}

    # Take in count the obstacle dimension (assuming another robot)
    # + the robot size.
    OBSTACLES_DIMENSION = 50

    def __init__(self, position):
        """
        position: init position dict with keys:
            - "angle": a X-axis relative angle.
            - "point": the first position.
        """
        self._position = position

        #  self._us_sensors = RangeSensor(4)
        self._motors = Motors(5)
        self._kinematic = Kinematics(6)

        logging.info('Building the graph map.')
        robot_diagonal = math.sqrt(self.DIMENSION['length'] +
                                   self.DIMENSION['width']**2)
        self._graph = build_graph(robot_diagonal)
        logging.info('Finished to build the graph map.')

    def take_modules(self, number=1, distance=0):
        for i in range(number):
            self._kinematic.down_clamp()
            time.sleep(self._DELAY_UP_DOWN_CLAMP)
            self._motors.forward(distance)
            while not self._motors.is_done(self.__done_callback):
                time.sleep(0.5)
            self._kinematic.close_clamp()
            time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
            self._motors.backward(distance)
            while not self._motors.is_done(self.__done_callback):
                time.sleep(0.5)

            #SEULEMENT SI ON DECIDE DE RECULER POUR MIEUX PRENDRE LE MODULE ---
            self._kinematic.open_clamp()
            time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
            self._motors.forward(2.5)
            while not self._motors.is_done(self.__done_callback):
                time.sleep(0.5)
            self._kinematic.close_clamp()
            time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
            #---

            self._kinematic.up_clamp()
            time.sleep(self._DELAY_UP_DOWN_CLAMP)
            self._kinematic.open_clamp()

    def eject_modules(self, number=1):
        for i in range(number):
            self._kinematic.push_out()
            time.sleep(self._DELAY_IN_OUT_BLOCK)
            if number == 4 and i == 2:
                self._kinematic.open_clamp()
                time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
            self._kinematic.push_back()
            time.sleep(self._DELAY_IN_OUT_BLOCK)

    def move_to(self, target):
        """
        target: a dict with keys:
            - "angle": a X-axis relative constrain target angle.
            - "point": position of the target.
        """
        while True:
            instructions = self._graph.get_path(self._position, target)
            status = self._motors.move_with_instructions(
                instructions, self.__move_callback, self.__done_callback)
            if status == 'ok':
                break

    def finalize(self):
        self._motors.stop()

        self._kinematic.launch_funny()
        time.sleep(1)
        self._kinematic.reset_funny()

    def __move_callback(self):
        """
        Return a string giving the state if we need to stop or not the regulation because of
        an obstacle.
        """
        #  ranges = self._us_sensors.get_ranges()
        ranges = [20, 20, 20, 20, 20]

        for us_data in self.US_SENSORS:
            for i, sensor in enumerate(us_data['sensors']):
                if ranges[sensor] == 0:
                    # Zero value means that we are too far from obstacles.
                    continue
                if ranges[sensor] < us_data['trigger_limit']:
                    logging.warn(
                        'Motors stopped becauce of the %s%i US sensors at %i cm',
                        us_data['name'], i, ranges[sensor])
                    self._motors.stop()
                    # TODO: avoid recalculation if we have the near same position.
                    self._graph.reset_obstacles()
                    self._graph.add_obstacle(self._position, self.DIMENSION,
                                             self.OBSTACLES_DIMENSION,
                                             us_data['name'], ranges[sensor])
                    return 'obstacle'
        return 'continue'

    def __done_callback(self, distance_travelled, status='ok'):
        """
        Update the position and angle informations of the robots.
        """
        self._move_target = None

        logging.info('Regulation is done!')
        self.__update_robot_position(distance_travelled)
        print(self._position)

        return status

    def __update_robot_position(self, distance_travelled):
        left = distance_travelled['left']
        right = distance_travelled['right']

        if self.__is_opposite_sign(left, right):
            # We finished a rotation regulation.
            angle = ((right - left) / 2) / Motors.ANGLE_CORRECTION
            self._position['angle'] += angle
            logging.info('Robot turned with an angle of %i', angle)
        else:
            # We finished a lead regulation.
            angle = self._position['angle']
            distance = (left + right) / 2

            self._position['point'][0] += distance * math.cos(
                math.radians(angle))
            self._position['point'][1] += distance * math.sin(
                math.radians(angle))

    def __is_opposite_sign(self, n, m):
        if n > 0 and m > 0:
            return False
        elif n < 0 and m < 0:
            return False
        return True
Esempio n. 26
0
def dancer():
    ZumoButton().wait_for_press()
    m = Motors()
    m.forward(.2, 3)
Esempio n. 27
0
File: t2.py Progetto: mattvenn/v2
board.connect()
move = Motors()
mission = Mission()

mission.startMission()
board.connect()
print("Board connected")
sleep(0.5)

del1 = 5  # This is standard delay 1 in seconds
del2 = 3  # This is standard delay 2 in seconds
speed = 50  # This takes a value from 0 to 254 

print("Will now go forward speed 50 for 1S")
sleep(0.8)
move.forward(50)
sleep(1)
move.stop()
print("Will now go backwards speed 50 for 1S")
sleep(1)
move.backward(50)
sleep(1)
move.stop()

print("robot will now move forward by 60 cm  on button press")
sleep(1)
mission.startMission()
sleep(1)
move.position(60,100)
sleep(5)
move.stop()
Esempio n. 28
0
from mission import Mission
board = Arduino()
board.connect()
move = Motors()

mission = Mission()

power_f = 100
power_t = 100

mission.startMission()
while True:
        
	
	print("Forward") 
	move.forward(power_f)
	sleep(4)
	move.stop()

	print("turn left")
	move.leftMotor(power_f, 0)
	move.rightMotor(power_f, 1)
	sleep(1)
	move.stop()
	sleep(1)

	print("turn right")
	move.leftMotor(power_f, 1)
	move.rightMotor(power_f, 0)
	sleep(1)
	move.stop()
Esempio n. 29
0
from motors import Motors

connection = SerialManager(device='/dev/ttyUSB0')
try:
    a = ArduinoApi(connection=connection)
except Exception as e:
    print('ERROR CONNECTING TO ARDUINO')
    exit(1)

motors = Motors()
motors.init_motors(a)

start = input('Any key to start')

print('Right wheel forward')
motors.forward(a, 0, 200)
sleep(1)
print('stopping...')
motors.stop(a)
sleep(.5)
print('Right wheel backward')
motors.reverse(a, 0, 200)
sleep(1)
print('stopping...')
motors.stop(a)
sleep(.5)
start = input('Any key to start')

print('Left wheel forward')
motors.forward(a, 200, 0)
sleep(1)
Esempio n. 30
0
class Mission():
    
    # tested
    def __init__(self):

        if pi is True:
            self.board = Arduino()
            self.board.connect()
            self.move = Motors()
            GPIO.setup(GREEN_LED_GPIO,GPIO.OUT)
            GPIO.setup(RED_LED_GPIO,GPIO.OUT)
            GPIO.setup(BUTTON, GPIO.IN, pull_up_down=GPIO.PUD_UP)

        # load the sample hash
        with open(rfid_hash) as fh:
            self.rfid_hash = json.load(fh)


    # won't test
    def startMission(self):
        if pi is True:
            GPIO.output(GREEN_LED_GPIO,False)
            GPIO.output(RED_LED_GPIO,True)
            print("waiting for button...")
            while GPIO.input(BUTTON) == False:
                time.sleep(0.1)
            GPIO.output(RED_LED_GPIO,False)

    # won't test
    def endMission(self):
        if pi is True:
            GPIO.output(GREEN_LED_GPIO,True)

    # tested
    def deleteData(self):
        try:
            os.unlink(data_file)
        except OSError:
            pass

    # tested
    # stores a dict as json on the disk
    # appends to the data file
    def saveData(self, sample):
        with open(data_file,'a') as fh:
            fh.write(json.dumps(sample) + "\n")

    # tested
    # returns a list of dicts
    def loadData(self):
        data = []
        try:
            with open(data_file,) as fh:
                for sample in fh.readlines():
                    data.append(json.loads(sample))
        except IOError:
            pass
        return data

    # won't test
    # fetches an RFID
    def getLocation(self):
        if pi is True:
            rfid = ""
            # keep moving until robot gets a location
            while True:
                rfid = self.board.sendCommand(Commands.READ_RFID,0,0)
                # when I catalogued the RFIDs I missed the last char!
                rfid = rfid[0:11]
                if len(rfid) == 11:
                    break
                self.move.forward(70)  
                time.sleep(0.1)
                self.move.stop()
                time.sleep(1)
            return rfid
    
    # tested
    # indexes into the sample database with the RFID and returns sample as dict
    def takeSample(self, location):
        try:
            return self.rfid_hash[location]
        except KeyError:
            raise Exception("unknown location [%s]" % location)

    # tested
    # uploads a sample dict with a team name (string)
    def uploadSample(self, sample, team):
        # fetch team ID from nane
        r = requests.get(mc_url + '/api/team/' + team)
        if r.status_code == 400:
            raise Exception(r.text)

        team_id = json.loads(r.text)['id']
        sample['team'] = str(team_id)

        # posts it
        r = requests.post(mc_url + '/api/sample', json=sample)
        if r.status_code == 400:
            return r.text
            #exeception stops the program
            #raise Exception(r.text)

        new_sample = json.loads(r.text)
        print("uploaded sample %d" % new_sample['id'])
        return new_sample
    
    def getAllSamples(self):
        r = requests.get(mc_url + '/api/samples')
        samples = json.loads(r.text)['samples']
        return samples
Esempio n. 31
0
class Motob:

    def __init__(self):
        self.motor = Motors()
        self.value = -1
        self.speed = 0.35
        self.funcs = {
            "left": self.left,
            "right": self.right,
            "random": self.random,
            "rewind": self.rewind
        }

    def update(self, recommendation):
        print ('operating',recommendation,'in motobs')
        self.operate(recommendation)

    def operate(self, recommended):
        if recommended == "None":
            self.wander()
        else:
            func, self.value = recommended.split(" ")
            self.value = int(self.value)
            if func in self.funcs:
                self.funcs[func]()
            else:
                self.wander()

    def wander(self):
        self.motor.forward(speed = self.speed, dur = 0.15)

    def degree_to_duration(self, degrees):
        # 0.75 per runde på full speed
        # anta halv speed, så ca 1.5 sec per runde
        time_per_round = 2.5
        time_per_degree = time_per_round/360
        return degrees*time_per_degree

    def get_degrees(self):
        return self.degree_to_duration(self.value)

    def left(self):
        self.motor.left(speed=self.speed*2, dur=self.get_degrees())

    def right(self):
        self.motor.right(speed=self.speed*2, dur=self.get_degrees())

    def random(self):
        # turn left or right by random
        # turn 90 degrees regardless
        self.value = 90
        if randint(0,100)>50:
            self.right()
        else:
            self.left()

    def rewind(self):
        # run back for a while
        if self.value <= 0:
            self.value = 1      # set a default value
        self.motor.backward(speed=self.speed, dur = self.value)