Esempio n. 1
1
def main(robot):
    # Définition des moteurs / capteurs
    m_left = Motor(robot, PORT_B)
    m_right = Motor(robot, PORT_A)

    touch_right = Touch(robot, PORT_2)
    touch_left = Touch(robot, PORT_1)
    ultrason = Ultrasonic(robot, PORT_3)

    DEFAULT_POWER = 80
    DISTANCE_LIMIT = 20
    TURN_TIME = 1.5

    # Début
    while True:
        m_left.run(power=DEFAULT_POWER)
        m_right.run(power=DEFAULT_POWER)

        while not touch_right.is_pressed() and not touch_left.is_pressed() and ultrason.get_distance() > DISTANCE_LIMIT:
            sleep(0.01)

        print("Aieee")
        if touch_right.is_pressed():
            m_left.run(power=DEFAULT_POWER)
            m_left.run(power=-DEFAULT_POWER)
        else:
            m_left.run(power=-DEFAULT_POWER)
            m_left.run(power=DEFAULT_POWER)

        sleep(TURN_TIME)
Esempio n. 2
0
def main(robot):
    # Définition des moteurs / capteurs
    m_left = Motor(robot, PORT_B)
    m_right = Motor(robot, PORT_A)

    touch_right = Touch(robot, PORT_2)
    touch_left = Touch(robot, PORT_1)
    ultrason = Ultrasonic(robot, PORT_3)

    DEFAULT_POWER = 80
    DISTANCE_LIMIT = 20
    TURN_TIME = 1.5

    # Début
    while True:
        m_left.run(power=DEFAULT_POWER)
        m_right.run(power=DEFAULT_POWER)

        while not touch_right.is_pressed() and not touch_left.is_pressed(
        ) and ultrason.get_distance() > DISTANCE_LIMIT:
            sleep(0.01)

        print("Aieee")
        if touch_right.is_pressed():
            m_left.run(power=DEFAULT_POWER)
            m_left.run(power=-DEFAULT_POWER)
        else:
            m_left.run(power=-DEFAULT_POWER)
            m_left.run(power=DEFAULT_POWER)

        sleep(TURN_TIME)
Esempio n. 3
0
 def initialize(self):
     super(DrivarNxt, self).initialize()
     self.m_block = nxt.locator.find_one_brick()
     self.m_leftMotor = Motor(self.m_block, PORT_A)
     self.m_rightMotor = Motor(self.m_block, PORT_C)
     self.m_ultrasonicSensor = Ultrasonic(self.m_block, PORT_4)
     self.m_initialized = True
Esempio n. 4
0
 def initialize(self):
     super(DrivarNxt, self).initialize()
     self.block = nxt.locator.find_one_brick()
     self.leftMotor = Motor(self.block, PORT_A)
     self.rightMotor = Motor(self.block, PORT_C)
     self.penMotor = Motor(self.block, PORT_B)
     self.ultrasonicSensor = Ultrasonic(self.block, PORT_4)
     self.lightSensor = Light(self.block, PORT_3)
     self.initialized = True
Esempio n. 5
0
def ConnectToNxt():
    brick = nxt.bluesock.BlueSock('00:16:53:04:17:F1').connect()

    motor_right = Motor(brick, PORT_A)
    motor_left = Motor(brick, PORT_C)
    ultrasonic_front = Ultrasonic(brick, PORT_2)
    ultrasonic_left = Ultrasonic(brick, PORT_1)
    ultrasonic_right = Ultrasonic(brick, PORT_3)

    return motor_right, motor_left, ultrasonic_front, ultrasonic_left, ultrasonic_right
Esempio n. 6
0
class Robot:
    
    def __init__(self):
        print 'Searching for NXT bricks...'
        self.robot = nxt.locator.find_one_brick()
        print 'NXT brick found'
        self.right_motor = Motor(self.robot, PORT_B)
        self.left_motor = Motor(self.robot, PORT_C)
        self.locator = Ultrasonic(self.robot, PORT_1)
        self.haptic = Touch(self.robot, PORT_4)
        
    def forward(self):
        
        if(random.random() > .5):
            self.right_motor.run(-STRAIGHT_POWER)
            self.left_motor.run(-STRAIGHT_POWER)
        else:
            self.left_motor.run(-STRAIGHT_POWER)
            self.right_motor.run(-STRAIGHT_POWER)
        
        sleep(SECONDS)
        
        if(random.random() > .5):
            self.right_motor.idle()
            self.left_motor.idle()
        else:
            self.left_motor.idle()
            self.right_motor.idle()
        
    def back(self):
        
        self.right_motor.run(STRAIGHT_POWER)
        self.left_motor.run(STRAIGHT_POWER)
        
        sleep(SECONDS)
        
        self.right_motor.idle()
        self.left_motor.idle()
        
    def right(self):
        self.left_motor.turn(-TURN_POWER, ANGLE)
     
    def left(self):
        self.right_motor.turn(-TURN_POWER, ANGLE)
        
    def distance(self):
        return self.locator.get_sample()
    
    def is_touching(self):
        return self.haptic.get_sample()
    
    def beep_ok(self):
        self.robot.play_tone_and_wait(FREQ_C, DURATION)
        self.robot.play_tone_and_wait(FREQ_D, DURATION)
        self.robot.play_tone_and_wait(FREQ_E, DURATION)
        
    def beep_not_ok(self):
        self.robot.play_tone_and_wait(FREQ_E, DURATION)
        self.robot.play_tone_and_wait(FREQ_D, DURATION)
        self.robot.play_tone_and_wait(FREQ_C, DURATION)
Esempio n. 7
0
class Car:

    brick = None
    left = None
    right = None
    
    def __init__ (self):  
        brick = nxt.locator.find_one_brick()
        self.brick = brick
        self.left = Motor(brick, PORT_A)
        self.right = Motor(brick, PORT_C)
        self.light = Light(brick, PORT_3)
        self.ultrasonic = Ultrasonic(brick, PORT_4)
        print "Connection established."

    def turn (self, angle=100, speed = 30):
        if angle > 0:
            self.left.turn(angle, speed)
        elif angle < 0:
            self.right.turn(-angle, speed)

    def forward (self, distance=50, speed=30):
        self.left.run(power=speed)
        self.right.run(power=speed)
        sleep(distance/speed)
        self.left.idle()
        self.right.idle()
    
    def range(self):
        return self.ultrasonic.get_sample()

    def surface(self):
        return self.light.get_sample()
Esempio n. 8
0
 def initialize(self):
     super(DrivarNxt,self).initialize()
     self.m_block = nxt.locator.find_one_brick()
     self.m_leftMotor = Motor(self.m_block, PORT_A)
     self.m_rightMotor = Motor(self.m_block, PORT_C)
     self.m_ultrasonicSensor = Ultrasonic(self.m_block, PORT_4)
     self.m_initialized = True
Esempio n. 9
0
def main():
    b = find_one_brick()
    eyes = Motor(b, PORT_A)
    wheels = [Motor(b, PORT_B), Motor(b, PORT_C)]
    ultrasonic = Ultrasonic(b, PORT_4)

    phizic(eyes, wheels, ultrasonic)
Esempio n. 10
0
 def getDistance(self, port):
     if self._bricks:
         try:
             port = int(port)
         except:
             pass
         if (port in NXT_SENSOR_PORTS):
             try:
                 port_aux = NXT_SENSOR_PORTS[port]
                 sensor = Ultrasonic(self._bricks[self.active_nxt], port_aux)
                 return sensor.get_sample()
             except:
                 return ERROR
         else:
             raise logoerror(ERROR_PORT_S % port)
     else:
         raise logoerror(ERROR_BRICK)
Esempio n. 11
0
 def __init__ (self):  
     brick = nxt.locator.find_one_brick()
     self.brick = brick
     self.left = Motor(brick, PORT_A)
     self.right = Motor(brick, PORT_C)
     self.light = Light(brick, PORT_3)
     self.ultrasonic = Ultrasonic(brick, PORT_4)
     print "Connection established."
Esempio n. 12
0
 def __init__(self):
     print 'Searching for NXT bricks...'
     self.robot = nxt.locator.find_one_brick()
     print 'NXT brick found'
     self.right_motor = Motor(self.robot, PORT_B)
     self.left_motor = Motor(self.robot, PORT_C)
     self.locator = Ultrasonic(self.robot, PORT_1)
     self.haptic = Touch(self.robot, PORT_4)
Esempio n. 13
0
 def getCompass(self, port):
     if self._bricks:
         try:
             port = int(port)
         except:
             pass
         if (port in NXT_SENSOR_PORTS):
             try:
                 port_aux = NXT_SENSOR_PORTS[port]
                 sensor = Ultrasonic(self._bricks[self.active_nxt], port_aux)
                 return sensor.get_sample()
             except:
                 return ERROR
         else:
             raise logoerror(ERROR_PORT_S % port)
     else:
         raise logoerror(ERROR_BRICK)
Esempio n. 14
0
    def __init__(self, brick='NXT'):
        r'''Creates a new Alpha Rex controller.

            brick
                Either an nxt.brick.Brick object, or an NXT brick's name as a
                string. If omitted, a Brick named 'NXT' is looked up.
        '''
        if isinstance(brick, basestring):
            brick = find_one_brick(name=brick)

        self.brick = brick
        self.arms = Motor(brick, PORT_C)
        self.left = Motor(brick, PORT_A)
        self.right = Motor(brick, PORT_B)

        self.direction = HTCompass(brick, PORT_2)
        self.ultrasonic = Ultrasonic(brick, PORT_4)
Esempio n. 15
0
 def see(self, ultrasonicPort='PORT_4', ultrasonicDistance=25, operator='<', initial=False):
     """
     Use the ultrasonic sensor to see obstacles (0-255) with 25 being about a foot away
     
     ultrasonicPort (str): The port used for the ultrasonic sensor (default='PORT_4')
     ultrasonicDistance (int): The distance that the ultrasonic returns true [0-255, 25~1foot] (default=25)
     operator (str): The operator for comparing distance ['<','>','=','!='] (default='<')
     """
     
     ultrasonic=Ultrasonic(self.nxt.brick, eval(ultrasonicPort))
     if initial:
         return False                    #don't return true until actually checked sensor value
     else:
         data = ultrasonic.get_sample()  #integer between 0 and 255
         output = operation(data,operator,ultrasonicDistance)
         if output:
             print 'Ultrasonic Distsance is '+operator+' '+str(ultrasonicDistance)
         return output
Esempio n. 16
0
    def __init__(self, brick='NXT'):
        r'''Creates a new Alpha Rex controller.
        
            brick
                Either an nxt.brick.Brick object, or an NXT brick's name as a
                string. If omitted, a Brick named 'NXT' is looked up.
        '''
        if isinstance(brick, str):
            brick = find_one_brick(name=brick)

        self.brick = brick
        self.arms = Motor(brick, PORT_A)
        self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)]

        self.touch = Touch(brick, PORT_1)
        self.sound = Sound(brick, PORT_2)
        self.light = Light(brick, PORT_3)
        self.ultrasonic = Ultrasonic(brick, PORT_4)
Esempio n. 17
0
 def __init__(self, world):
     Robot.__init__(self, world, 2, Orientation.NORTH)
     self.noise = 0
     self.brick = find_one_brick()
     self.motor1 = Motor(self.brick, PORT_B)
     self.motor2 = Motor(self.brick, PORT_C)
     self.ultrasonic = Ultrasonic(self.brick, PORT_4)
     self.motor1.reset_position(False)
     self.motor2.reset_position(False)
Esempio n. 18
0
 def see(self, ultrasonicPort='PORT_4', ultrasonicDistance=25, operator='<', initial=False):
     """
     Use the ultrasonic sensor to see obstacles (0-255) with 25 being about a foot away
     
     ultrasonicPort (str): The port used for the ultrasonic sensor (default='PORT_4')
     ultrasonicDistance (int): The distance that the ultrasonic returns true [0-255, 25~1foot] (default=25)
     operator (str): The operator for comparing distance ['<','>','=','!='] (default='<')
     """
     
     ultrasonic=Ultrasonic(self.nxt.brick, eval(ultrasonicPort))
     if initial:
         return False                    #don't return true until actually checked sensor value
     else:
         data = ultrasonic.get_sample()  #integer between 0 and 255
         output = operation(data,operator,ultrasonicDistance)
         if output:
             print 'Ultrasonic Distsance is '+operator+' '+str(ultrasonicDistance)
         return output
Esempio n. 19
0
 def getDistance(self, port):
     if self._bricks:
         time.sleep(0.5)
         try:
             port = int(port)
         except:
             pass
         if (port in NXT_SENSOR_PORTS):
             res = ERROR
             #try:
             port_aux = NXT_SENSOR_PORTS[port]
             sensor = Ultrasonic(self._bricks[self.active_nxt], port_aux)
             res = sensor.get_sample()
             #except:
             #pass
             return res
         else:
             pass
     else:
         pass
Esempio n. 20
0
    def __init__(self):
        self.brick = nxt.bluesock.BlueSock('00:16:53:08:51:40').connect()
        self.right = Motor(self.brick, PORT_A)
        self.left = Motor(self.brick, PORT_C)
        self.legs = Legs(self.right, self.left)
        
        #self.ear = Sound(self.brick, PORT_2)
        self.sonic = Ultrasonic(self.brick, PORT_4)

        self.owner_distance = 0

        self.stamina = 200
Esempio n. 21
0
    def __init__(self):
        self.brick = nxt.bluesock.BlueSock("00:16:53:08:51:40").connect()
        # self.brick = nxt.locator.find_one_brick()
        self.right = Motor(self.brick, PORT_A)
        self.left = Motor(self.brick, PORT_C)
        self.legs = Legs(self.right, self.left)

        # self.ear = Sound(self.brick, PORT_2)
        self.sonic = Ultrasonic(self.brick, PORT_4)

        self.owner_distance = 0

        self.last_distance = 255
Esempio n. 22
0
    def __init__(self, brick='NXT'):
        r'''Creates a new Alpha Rex controller.
        
            brick
                Either an nxt.brick.Brick object, or an NXT brick's name as a
                string. If omitted, a Brick named 'NXT' is looked up.
        '''
        if isinstance(brick, str):
            brick = find_one_brick(name=brick)
    
        self.brick = brick
        self.arms = Motor(brick, PORT_A)
        self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)]

        self.touch = Touch(brick, PORT_1)
        self.sound = Sound(brick, PORT_2)
        self.light = Light(brick, PORT_3)
        self.ultrasonic = Ultrasonic(brick, PORT_4)
Esempio n. 23
0
    def __init__(self, brick="NXT"):
        r"""Creates a new Robot controller.
        
            brick
                Either an nxt.brick.Brick object, or an NXT brick's name as a
                string. If omitted, a Brick named 'NXT' is looked up.
        """
        if isinstance(brick, basestring):
            brick = find_one_brick(name=brick)

        self.brick = brick
        self.tool = Motor(brick, PORT_B)
        self.tracks = [Motor(brick, PORT_A), Motor(brick, PORT_C)]

        self.ultrasonic = Ultrasonic(brick, PORT_1)
        self.sound = Sound(brick, PORT_2)
        self.light = Light(brick, PORT_3)
        self.touch = Touch(brick, PORT_4)
Esempio n. 24
0
 def __init__(self, brick="NXT"):
     if isinstance(brick, basestring):
         brick = find_one_brick(name=brick)
     self.motor = True
     self.sensor = True
     try:
         self.brick = brick
     except:
         print("No brick found")
     try:
         self.wheels = [Motor(brick, PORT_A), Motor(brick, PORT_B)]
     except:
         self.motor = False
         print("No motors detected!")
     try:
         self.bat_eyes = Ultrasonic(brick, PORT_1)
     except Exception as e:
         self.sensor = False
         print(e)
         print("No sensor found")
Esempio n. 25
0
    # the bluetooth function of the nxt library works too, but "wastes"
    # time searching for devices.
    brick = nxtConnect.btConnect(brickName)
    
print(brick.get_device_info()) # check what brick you connected to
from time import sleep

from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4, PORT_3, PORT_2, Light, PORT_1, Ultrasonic

light = Light(brick, PORT_4)
turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
armMotor = Motor(brick, PORT_A)
touch = Touch(brick, PORT_1)
ultrasonic = Ultrasonic(brick, PORT_2)
compass = Ultrasonic(brick, PORT_3)

#TODO
# calibrate sleep times might need adjusting
# find bin is probably non functional, and VERY necessary (very short range when picking up bin)
# additional bin ID data to get more accurate values
# line follow data

# LINE FOLLOW VARIABLES
turningPower = 70 # 70, motor power used when turning in line follow
negInertiaPower = 65 # 65, motor power for negative inertia
findLineTimeOut = 0.5 # 0.5, time between switching motor to the opposite direction
negInertiaLengthOnWhite = 0.2 # 0.2, time before braking on negative inertia when originally on white
negInertiaLengthOnBlack = 0.05 # 0.05, time before braking on negative inertia when originally on black (should be smaller than white to prevent overshooting the line)
Esempio n. 26
0
    brick = nxtConnect.btConnect(brickName)
    
print(brick.get_device_info()) # check what brick you connected to
from time import sleep

from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4, PORT_3, PORT_2, Light, PORT_1, Ultrasonic

#from basicFunctions import step, calibrate

light = Light(brick, PORT_1)
turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
armMotor = Motor(brick, PORT_A)
legPosition = Touch(brick, PORT_3)
ultrasonic = Ultrasonic(brick, PORT_4)

def step(forwardPower):
    #print('stepping')
    walkingMotor.run(power = forwardPower)
    sleep(.1)
    while True:
        if legPosition.is_pressed() == True:
            walkingMotor.run(power = 0)
            walkingMotor.brake()
            return
            
def lineFollow():
    step(120)
    pass
Esempio n. 27
0
class DrivarNxt(Drivar):
    
    def __init__(self):
        self.m_initialized = False
        self.m_block = None
        self.m_leftMotor = None
        self.m_rightMotor = None
        self.m_ultrasonicSensor = None
        self.m_moving = False

    def initialize(self):
        super(DrivarNxt,self).initialize()
        self.m_block = nxt.locator.find_one_brick()
        self.m_leftMotor = Motor(self.m_block, PORT_A)
        self.m_rightMotor = Motor(self.m_block, PORT_C)
        self.m_ultrasonicSensor = Ultrasonic(self.m_block, PORT_4)
        self.m_initialized = True
        

    def move(self, direction=Drivar.DIR_FORWARD,durationInMs=1000, callback = None):
        durationInMs = max(durationInMs,100)
        _direct = direction
        self.rotateWheels(direction = _direct)
        time.sleep(durationInMs/1000)
        self.stop()
        if callback is not None:
            callback()
    
    def rotateWheels(self, wheelSet = Drivar.WHEELS_BOTH, direction = Drivar.DIR_FORWARD, speedLevel = Drivar.SPEED_FAST, callback = None):
        power = self._getNxtSpeed(speedLevel)
        # Correct the power (positive vs negative) depending on the direction
        if(direction == Drivar.DIR_FORWARD):
            if(power < 0):
                power = power * -1
        if(direction == Drivar.DIR_BACKWARD):
            if(power > 0):
                power = power * -1
        # Get the wheels turning
        if(wheelSet == Drivar.WHEELS_LEFT or wheelSet == Drivar.WHEELS_BOTH):
            self.m_leftMotor.run(power)
        if(wheelSet == Drivar.WHEELS_RIGHT or wheelSet == Drivar.WHEELS_BOTH):
            self.m_rightMotor.run(power)
        self.m_moving = True
        if callback is not None:
            callback()
        
    def turn(self, direction = Drivar.DIR_LEFT, angle = 90):
        left_power = -100
        right_power = 100
        if(direction == Drivar.DIR_RIGHT):
            left_power *= -1 
            right_power *= -1
        self.m_leftMotor.turn(left_power, angle)
        self.m_rightMotor.turn(right_power, angle)


    
    
    def stop(self, callback = None):
        self.m_leftMotor.idle()
        self.m_rightMotor.idle()
        self.m_moving = False
        if callback is not None:
            callback()
        
 
 
    '''
      Return the distance to the nearest obstacle, in centimeters
    '''
    def getDistanceToObstacle(self):
        return self.m_ultrasonicSensor.get_sample()
 
    '''
      Indicate with a boolean whether there is an obstacle within the given distance
    '''
    def isObstacleWithin(self, distance):
        dist = self.m_ultrasonicSensor.get_sample()
        if(dist <= distance):
            return True
        else:
            return False
        
    '''
      Return the NXT speed equivalent for the given DRIVAR speed flag
    '''
    @staticmethod
    def _getNxtSpeed(speed):
        if(speed==Drivar.SPEED_SLOW):
            return 70
        elif(speed==Drivar.SPEED_MEDIUM):
            return 100
        elif(speed==Drivar.SPEED_FAST):
            return 127
        else :
            return 100 
Esempio n. 28
0
class Seng(object):
    def __init__(self, brick='NXT'):
        r'''Creates a new Alpha Rex controller.

            brick
                Either an nxt.brick.Brick object, or an NXT brick's name as a
                string. If omitted, a Brick named 'NXT' is looked up.
        '''
        if isinstance(brick, basestring):
            brick = find_one_brick(name=brick)

        self.brick = brick
        self.arms = Motor(brick, PORT_C)
        self.left = Motor(brick, PORT_A)
        self.right = Motor(brick, PORT_B)

        self.direction = HTCompass(brick, PORT_2)
        self.ultrasonic = Ultrasonic(brick, PORT_4)

    def echolocate(self):
        r'''Reads the Ultrasonic sensor's output.
        '''
        return self.ultrasonic.get_sample()

    def compass(self):
        return self.direction.get_sample()

    def walk_seng(self, secs, power):
        r'''Simultaneously activates the leg motors, causing Alpha Rex to walk.

            secs
                How long the motors will rotate.

            power
                The strength effected by the motors. Positive values will cause
                Alpha Rex to walk forward, while negative values will cause it
                to walk backwards. If you are unsure about how much force to
                apply, the special values FORTH and BACK provide reasonable
                defaults. If omitted, FORTH is used.
        '''

        self.left.run(power=power)
        self.right.run(power=power)

        sleep(secs)
        self.left.idle()
        self.right.idle()

    def walk_seng_start(self, power):
        self.left.run(power=power)
        self.right.run(power=power)

    def walk_seng_stop(self):
        self.left.idle()
        self.right.idle()

    def turn_seng(self, secs, power):
        self.left.run(power=power)
        self.right.run(power=-power)

        sleep(secs)
        self.left.idle()
        self.right.idle()

    def turn_seng_start(self, power):
        self.left.run(power=power)
        self.right.run(power=-power)


    def turn_seng_stopp(self):
        self.left.idle()
        self.right.idle()
Esempio n. 29
0
 def ultrasonic(self):
     us = Ultrasonic(self.brick, PORT_3)
     return us.get_distance()
Esempio n. 30
0
class Dog(object):

    MIN_OWNER_DISTANCE = 30
    MAX_OWNER_DISTANCE = 40
    LOST_DISTANCE = 100

    def __init__(self):
        self.brick = nxt.bluesock.BlueSock("00:16:53:08:51:40").connect()
        # self.brick = nxt.locator.find_one_brick()
        self.right = Motor(self.brick, PORT_A)
        self.left = Motor(self.brick, PORT_C)
        self.legs = Legs(self.right, self.left)

        # self.ear = Sound(self.brick, PORT_2)
        self.sonic = Ultrasonic(self.brick, PORT_4)

        self.owner_distance = 0

        self.last_distance = 255

    @property
    def distance_ahead(self):
        sample = self.sonic.get_distance()
        if sample == 255:
            self.last_distance += 20
            return self.last_distance
        self.last_distance = sample
        return sample

    def wait(self):
        print "Waiting"

        self.stop()

        while True:
            distance = self.distance_ahead
            if distance > 30 and distance < 40:
                self.owner_distance = distance
                print "owner distance: %d" % distance
                return self.follow()

    def follow(self):
        print "Following"

        while True:
            distance = self.distance_ahead

            if distance > self.owner_distance + 50:
                return self.search()

            if distance > self.owner_distance:
                self.legs.walk(50 + distance - self.owner_distance)
            elif distance < self.owner_distance:
                self.legs.walk_back(50 + self.owner_distance - distance)
            else:
                self.stop()
                time.sleep(0.5)

    def search(self):
        print "Searching"

        angle = 0.5
        max_angle = 64
        direction = random.choice((-1, 1))

        while angle < max_angle:
            start = time.time()
            while self.distance_ahead > 100 and time.time() < start + angle:
                self.legs.turn(direction)
            self.legs.stop()
            if self.distance_ahead <= 100:
                return self.follow()
            direction *= -1
            angle *= 2

        return self.wait()

    def stop(self):
        self.legs.stop()
Esempio n. 31
0
print(brick.get_device_info())  # check what brick you connected to
from time import sleep
from nxt.sensor import Light, Touch, Ultrasonic
from nxt.sensor import PORT_1, PORT_2, PORT_3, PORT_4
from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
"""##########################################################################################
################################     IMPORT MOTORS AND SENSORS HERE     ################################
###########################################################################################"""

motorLeft = Motor(brick, PORT_B)
motorRight = Motor(brick, PORT_C)
armMotor = Motor(brick, PORT_A)
light = Light(brick, PORT_3)
touch = Touch(brick, PORT_4)
sonar = Ultrasonic(brick, PORT_2)
led = Light(brick, PORT_1)  # experimental
"""########################################################################################"""


def binIdent():

    n = 50

    #wait .25 seconds after the kill-switch is released
    sleep(.25)

    while n > 0:
        print("Bin Identification Running, power = %d" % n)
        # print(sonar.get_distance())
        armMotor.run(power=n)
Esempio n. 32
0
class Robot(object):
    def __init__(self, brick="NXT"):
        r"""Creates a new Robot controller.
        
            brick
                Either an nxt.brick.Brick object, or an NXT brick's name as a
                string. If omitted, a Brick named 'NXT' is looked up.
        """
        if isinstance(brick, basestring):
            brick = find_one_brick(name=brick)

        self.brick = brick
        self.tool = Motor(brick, PORT_B)
        self.tracks = [Motor(brick, PORT_A), Motor(brick, PORT_C)]

        self.ultrasonic = Ultrasonic(brick, PORT_1)
        self.sound = Sound(brick, PORT_2)
        self.light = Light(brick, PORT_3)
        self.touch = Touch(brick, PORT_4)

    def turn(self, power, angle):
        for motor in self.tracks:
            motor.turn(power, angle)

    def move(self, power=FORTH):
        r"""Simultaneously activates the tracks motors, causing Robot to move.
        
            power
                The strength effected by the motors. Positive values will cause
                Robot to move forward, while negative values will cause it
                to move backwards. If you are unsure about how much force to
                apply, the special values FORTH and BACK provide reasonable
                defaults. If omitted, FORTH is used.
        """
        for motor in self.tracks:
            motor.run(power=power)

    def wait(self, seconds):
        """ secsonds
                How long the motors will rotate.
                Will this take values < 0? Most motor commands work in ms. 
                Try passing sleep 1/seconds (miliseconds)
        """
        sleep(seconds)

    def stop(self):
        for motor in self.tracks:
            motor.idle()

    def tacho(self):
        """
           returns an array of two elements which are the motor tacho readings
        """
        tachos = []
        for motor in self.tracks:
            # tachos.append(motor.get_tacho())
            tachos.append(motor.tacho_count)  # , rotation_count

        return tachos

    def act(self, power=FORTH):
        r"""Make Robot move its tool.
        
            power
                The strength effected by the motor. If omitted, (100) is used.
        """
        self.tool.run(power=power)

    def echolocate(self):
        r"""Reads the Ultrasonic sensor's output.
        """
        return self.ultrasonic.get_sample()

    def feel(self):
        r"""Reads the Touch sensor's output.
        """
        return self.touch.get_sample()

    def hear(self):
        r"""Reads the Sound sensor's output.
        """
        return self.sound.get_sample()

    def say(self, line, times=1):
        r"""Plays a sound file named (line + '.rso'), which is expected to be
            stored in the brick. The file is played (times) times.

            line
                The name of a sound file stored in the brick.

            times
                How many times the sound file will be played before this method
                returns.
        """
        for i in range(0, times):
            self.brick.play_sound_file(False, line + ".rso")
            sleep(1)

    def see(self):
        r"""Reads the Light sensor's output.
        """
        return self.light.get_sample()
Esempio n. 33
0
class NxtRobot(Robot):
    FULL_ROTATE_FACTOR = 5.5
    CELL_MOVE_FACTOR = 2.35

    def __init__(self, world):
        Robot.__init__(self, world, 2, Orientation.NORTH)
        self.noise = 0
        self.brick = find_one_brick()
        self.motor1 = Motor(self.brick, PORT_B)
        self.motor2 = Motor(self.brick, PORT_C)
        self.ultrasonic = Ultrasonic(self.brick, PORT_4)
        self.motor1.reset_position(False)
        self.motor2.reset_position(False)

    def move(self, direction):
        if direction == Direction.FORWARD:
            pass
        elif direction == Direction.RIGHT:
            self._rotate(-0.25)
            pass
        elif direction == Direction.BACK:
            self._rotate(0.5)
            pass
        elif direction == Direction.LEFT:
            self._rotate(0.25)
            pass
        m = self.ultrasonic.get_sample()
        if m > 26:
            t1 = threading.Thread(target=self.motor1.turn, args=(50, self.CELL_MOVE_FACTOR * 360))
            t2 = threading.Thread(target=self.motor2.turn, args=(50, self.CELL_MOVE_FACTOR * 360))
            t1.start()
            t2.start()
            t1.join()
            t2.join()

    def measure(self):
        m = []
        for i in range(4):
            m.append(self.ultrasonic.get_sample())
            self._rotate(0.25)
        return m

    def sync(self):
        m = self.ultrasonic.get_sample()
        while m == 255:
            self._rotate(0.17)
            m = self.ultrasonic.get_sample()

    def _rotate(self, degree):
        if degree == 0:
            return
        sign = 1
        if degree < 0:
            sign = -1
            degree = abs(degree)
        t1 = threading.Thread(target=self.motor1.turn, args=(sign * 50, self.FULL_ROTATE_FACTOR * 360 * degree))
        t2 = threading.Thread(target=self.motor2.turn, args=(sign * -50, self.FULL_ROTATE_FACTOR * 360 * degree))
        t1.start()
        t2.start()
        t1.join()
        t2.join()
Esempio n. 34
0
def sonar_reading():
    x = [Ultrasonic(brick, PORT_1).get_sample(), 0]
    return x
Esempio n. 35
0
    # the bluetooth function of the nxt library works too, but "wastes"
    # time searching for devices.
    brick = nxtConnect.btConnect(brickName)
    
print(brick.get_device_info()) # check what brick you connected to
from time import sleep

from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4, PORT_3, PORT_2, Light, PORT_1, Ultrasonic

light = Light(brick, PORT_4)
turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
armMotor = Motor(brick, PORT_A)
touch = Touch(brick, PORT_1)
ultrasonic = Ultrasonic(brick, PORT_2)
compass = Ultrasonic(brick, PORT_3)

#turningMotor.run(70)


def legPosition():
    touchState = touch.get_input_values().calibrated_value
    if touchState == 829 or touchState == 810:
        return True
    else:
        return False
        
def step(forwardPower = 120):
    walkingMotor.run(forwardPower)
    sleep(.2) # give it time to move off touch sensor
Esempio n. 36
0
class DrivarNxt(Drivar):
    def __init__(self):
        self.m_initialized = False
        self.m_block = None
        self.m_leftMotor = None
        self.m_rightMotor = None
        self.m_penMotor = None
        self.m_ultrasonicSensor = None
        self.m_lightSensor = None
        self.m_moving = False

    def initialize(self):
        super(DrivarNxt, self).initialize()
        self.m_block = nxt.locator.find_one_brick()
        self.m_leftMotor = Motor(self.m_block, PORT_A)
        self.m_rightMotor = Motor(self.m_block, PORT_C)
        self.m_penMotor = Motor(self.m_block, PORT_B)
        self.m_ultrasonicSensor = Ultrasonic(self.m_block, PORT_4)
        self.m_lightSensor = Light(self.m_block, PORT_3)
        self.m_initialized = True

    def move(self,
             direction=Drivar.DIR_FORWARD,
             durationInMs=1000,
             callback=None):
        durationInMs = max(durationInMs, 100)
        _direct = direction
        self.rotateWheels(direction=_direct)
        time.sleep(durationInMs / 1000)
        self.stop()
        if callback is not None:
            callback()

    def rotateWheels(self,
                     wheelSet=Drivar.WHEELS_BOTH,
                     direction=Drivar.DIR_FORWARD,
                     speedLevel=Drivar.SPEED_FAST,
                     callback=None):
        power = self._getNxtSpeed(speedLevel)
        # Correct the power (positive vs negative) depending on the direction
        if (direction == Drivar.DIR_FORWARD):
            if (power < 0):
                power = power * -1
        if (direction == Drivar.DIR_BACKWARD):
            if (power > 0):
                power = power * -1
        # Get the wheels turning
        if (wheelSet == Drivar.WHEELS_LEFT or wheelSet == Drivar.WHEELS_BOTH):
            self.m_leftMotor.run(power)
        if (wheelSet == Drivar.WHEELS_RIGHT or wheelSet == Drivar.WHEELS_BOTH):
            self.m_rightMotor.run(power)
        self.m_moving = True
        if callback is not None:
            callback()

    def turn(self, direction=Drivar.DIR_LEFT, angle=90, callback=None):
        left_power = -100
        right_power = 100
        if (direction == Drivar.DIR_RIGHT):
            left_power *= -1
            right_power *= -1
        self.m_leftMotor.turn(left_power, angle)
        self.m_rightMotor.turn(right_power, angle)

    def stop(self, callback=None):
        self.m_leftMotor.idle()
        self.m_rightMotor.idle()
        self.m_moving = False
        if callback is not None:
            callback()

    '''
      Return the distance to the nearest obstacle, in centimeters
    '''

    def getDistanceToObstacle(self):
        return self.m_ultrasonicSensor.get_sample()

    '''
      Indicate with a boolean whether there is an obstacle within the given distance
    '''

    def isObstacleWithin(self, distance):
        dist = self.m_ultrasonicSensor.get_sample()
        if (dist <= distance):
            return True
        else:
            return False

    def rotatePen(self, angle):
        power = 70
        if angle < 0:
            angle = -1 * angle
            power = -70
        self.m_penMotor.turn(power, angle)

    def getReflectivityMeasurement(self):
        self.m_lightSensor.set_illuminated(True)
        return self.m_lightSensor.get_sample()

    def wait(self, milliseconds):
        time.sleep(milliseconds / 1000)

    '''
      Return the NXT speed equivalent for the given DRIVAR speed flag
    '''

    @staticmethod
    def _getNxtSpeed(speed):
        if (speed == Drivar.SPEED_SLOW):
            return 70
        elif (speed == Drivar.SPEED_MEDIUM):
            return 100
        elif (speed == Drivar.SPEED_FAST):
            return 127
        else:
            return 100
Esempio n. 37
0
def ultrasonic(port):
    u = Ultrasonic(b, SENSORS[port])
    return str(u.get_distance())
Esempio n. 38
0
class AlphaRex(object):
    r'''A high-level controller for the Alpha Rex model.
    
        This class implements methods for the most obvious actions performable
        by Alpha Rex, such as walk, wave its arms, and retrieve sensor samples.
        Additionally, it also allows direct access to the robot's components
        through public attributes.
    '''
    def __init__(self, brick='NXT'):
        r'''Creates a new Alpha Rex controller.
        
            brick
                Either an nxt.brick.Brick object, or an NXT brick's name as a
                string. If omitted, a Brick named 'NXT' is looked up.
        '''
        if isinstance(brick, str):
            brick = find_one_brick(name=brick)

        self.brick = brick
        self.arms = Motor(brick, PORT_A)
        self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)]

        self.touch = Touch(brick, PORT_1)
        self.sound = Sound(brick, PORT_2)
        self.light = Light(brick, PORT_3)
        self.ultrasonic = Ultrasonic(brick, PORT_4)

    def echolocate(self):
        r'''Reads the Ultrasonic sensor's output.
        '''
        return self.ultrasonic.get_sample()

    def feel(self):
        r'''Reads the Touch sensor's output.
        '''
        return self.touch.get_sample()

    def hear(self):
        r'''Reads the Sound sensor's output.
        '''
        return self.sound.get_sample()

    def say(self, line, times=1):
        r'''Plays a sound file named (line + '.rso'), which is expected to be
            stored in the brick. The file is played (times) times.

            line
                The name of a sound file stored in the brick.

            times
                How many times the sound file will be played before this method
                returns.
        '''
        for i in range(0, times):
            self.brick.play_sound_file(False, line + '.rso')
            sleep(1)

    def see(self):
        r'''Reads the Light sensor's output.
        '''
        return self.light.get_sample()

    def walk(self, secs, power=FORTH):
        r'''Simultaneously activates the leg motors, causing Alpha Rex to walk.
        
            secs
                How long the motors will rotate.
            
            power
                The strength effected by the motors. Positive values will cause
                Alpha Rex to walk forward, while negative values will cause it
                to walk backwards. If you are unsure about how much force to
                apply, the special values FORTH and BACK provide reasonable
                defaults. If omitted, FORTH is used.
        '''
        for motor in self.legs:
            motor.run(power=power)

        sleep(secs)

        for motor in self.legs:
            motor.idle()

    def wave(self, secs, power=100):
        r'''Make Alpha Rex move its arms.
        
            secs
                How long the arms' motor will rotate.
            
            power
                The strength effected by the motor. If omitted, (100) is used.
        '''
        self.arms.run(power=power)
        sleep(secs)
        self.arms.idle()
Esempio n. 39
0
class Dog(object):

    STATE_WALKING = 0
    STATE_SEARCHING = 2
    STATE_RESTING = 1
    
    def __init__(self):
        self.brick = nxt.bluesock.BlueSock('00:16:53:08:51:40').connect()
        self.right = Motor(self.brick, PORT_A)
        self.left = Motor(self.brick, PORT_C)
        self.legs = Legs(self.right, self.left)
        
        #self.ear = Sound(self.brick, PORT_2)
        self.sonic = Ultrasonic(self.brick, PORT_4)

        self.owner_distance = 0

        self.stamina = 200

    @property
    def distance_ahead(self):
        return self.sonic.get_distance()

    def tire(self):
        self.stamina -= 1
        
    def rest(self):
        self.state = self.STATE_RESTING
        print "Resting"

        self.stop()
        
        time.sleep(random.randint(30, 90))

        self.stamina = 200

        return self.walk()

    def walk(self):
        self.state = self.STATE_WALKING
        print "Walking"

        self.legs.walk()

        while True:
            if self.distance_ahead < 40:
                return self.search()

            self.tire()

            if self.stamina == 0:
                return self.rest()

    def search(self):
        self.state = self.STATE_SEARCHING
        print "Searching"
        
        angle = 0.5
        max_angle = 15
        direction = random.choice((-1, 1))

        while angle < max_angle:
            start = time.time()
            while self.distance_ahead > 100 and time.time() < start + angle:
                self.legs.turn(direction)
            self.legs.stop()
            if self.distance_ahead <= 100:
                return self.walk()
            direction *= -1
            angle *= 1.5

            self.tire()

            if self.stamina == 0:
                return self.rest()

        return self.walk()

    def stop(self):
        self.legs.stop()
Esempio n. 40
0
class AlphaRex(object):
    r'''A high-level controller for the Alpha Rex model.
    
        This class implements methods for the most obvious actions performable
        by Alpha Rex, such as walk, wave its arms, and retrieve sensor samples.
        Additionally, it also allows direct access to the robot's components
        through public attributes.
    '''
    def __init__(self, brick='NXT'):
        r'''Creates a new Alpha Rex controller.
        
            brick
                Either an nxt.brick.Brick object, or an NXT brick's name as a
                string. If omitted, a Brick named 'NXT' is looked up.
        '''
        if isinstance(brick, str):
            brick = find_one_brick(name=brick)
    
        self.brick = brick
        self.arms = Motor(brick, PORT_A)
        self.legs = [Motor(brick, PORT_B), Motor(brick, PORT_C)]

        self.touch = Touch(brick, PORT_1)
        self.sound = Sound(brick, PORT_2)
        self.light = Light(brick, PORT_3)
        self.ultrasonic = Ultrasonic(brick, PORT_4)

    def echolocate(self):
        r'''Reads the Ultrasonic sensor's output.
        '''
        return self.ultrasonic.get_sample()
    
    def feel(self):
        r'''Reads the Touch sensor's output.
        '''
        return self.touch.get_sample()

    def hear(self):
        r'''Reads the Sound sensor's output.
        '''
        return self.sound.get_sample()

    def say(self, line, times=1):
        r'''Plays a sound file named (line + '.rso'), which is expected to be
            stored in the brick. The file is played (times) times.

            line
                The name of a sound file stored in the brick.

            times
                How many times the sound file will be played before this method
                returns.
        '''
        for i in range(0, times):
            self.brick.play_sound_file(False, line + '.rso')
            sleep(1)

    def see(self):
        r'''Reads the Light sensor's output.
        '''
        return self.light.get_sample()

    def walk(self, secs, power=FORTH):
        r'''Simultaneously activates the leg motors, causing Alpha Rex to walk.
        
            secs
                How long the motors will rotate.
            
            power
                The strength effected by the motors. Positive values will cause
                Alpha Rex to walk forward, while negative values will cause it
                to walk backwards. If you are unsure about how much force to
                apply, the special values FORTH and BACK provide reasonable
                defaults. If omitted, FORTH is used.
        '''
        for motor in self.legs:
            motor.run(power=power)
        
        sleep(secs)

        for motor in self.legs:
            motor.idle()

    def wave(self, secs, power=100):
        r'''Make Alpha Rex move its arms.
        
            secs
                How long the arms' motor will rotate.
            
            power
                The strength effected by the motor. If omitted, (100) is used.
        '''
        self.arms.run(power=power)
        sleep(secs)
        self.arms.idle()
Esempio n. 41
0
class Robot(object):
    
    def __init__(self, brick='NXT'):
        r'''Creates a new Robot controller.
        
            brick
                Either an nxt.brick.Brick object, or an NXT brick's name as a
                string. If omitted, a Brick named 'NXT' is looked up.
        '''
        if isinstance(brick, basestring):
            brick = find_one_brick(name=brick)
    
        self.brick = brick
        self.tool = Motor(brick, PORT_B)
        self.tracks = [Motor(brick, PORT_A), Motor(brick, PORT_C)]

        
        self.ultrasonic = Ultrasonic(brick, PORT_1)
        self.sound = Sound(brick, PORT_2)
        self.light = Light(brick, PORT_3)
        self.touch = Touch(brick, PORT_4)
        
    def turn(self, power, angle):
        for motor in self.tracks:
            motor.turn(power, angle)

    def move(self, power=FORTH):
        r'''Simultaneously activates the tracks motors, causing Robot to move.
        
            power
                The strength effected by the motors. Positive values will cause
                Robot to move forward, while negative values will cause it
                to move backwards. If you are unsure about how much force to
                apply, the special values FORTH and BACK provide reasonable
                defaults. If omitted, FORTH is used.
        '''
        for motor in self.tracks:
            motor.run(power=power)
            
    def wait(self, seconds):
        ''' secsonds
                How long the motors will rotate.
                Will this take values < 0? Most motor commands work in ms. 
                Try passing sleep 1/seconds (miliseconds)
        '''    
        sleep(seconds)
        
    def stop(self):
        for motor in self.tracks:
            motor.idle()
            
    def tacho(self):
        '''
           returns an array of two elements which are the motor tacho readings
        '''
        tachos = []
        for motor in self.tracks:
            #tachos.append(motor.get_tacho())
            tachos.append(motor.tacho_count) #, rotation_count
            
        return tachos        

    def act(self, power=FORTH):
        r'''Make Robot move its tool.
        
            power
                The strength effected by the motor. If omitted, (100) is used.
        '''
        self.tool.run(power=power)


    def echolocate(self):
        r'''Reads the Ultrasonic sensor's output.
        '''
        return self.ultrasonic.get_sample()
    
    def feel(self):
        r'''Reads the Touch sensor's output.
        '''
        return self.touch.get_sample()

    def hear(self):
        r'''Reads the Sound sensor's output.
        '''
        return self.sound.get_sample()

    def say(self, line, times=1):
        r'''Plays a sound file named (line + '.rso'), which is expected to be
            stored in the brick. The file is played (times) times.

            line
                The name of a sound file stored in the brick.

            times
                How many times the sound file will be played before this method
                returns.
        '''
        for i in range(0, times):
            self.brick.play_sound_file(False, line + '.rso')
            sleep(1)

    def see(self):
        r'''Reads the Light sensor's output.
        '''
        return self.light.get_sample()
Esempio n. 42
0
    # the bluetooth function of the nxt library works too, but "wastes"
    # time searching for devices.
    brick = nxtConnect.btConnect(brickName)

print(brick.get_device_info())  # check what brick you connected to
from time import sleep

from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4, PORT_3, PORT_2, Light, PORT_1, Ultrasonic

light = Light(brick, PORT_4)
turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
armMotor = Motor(brick, PORT_A)
touch = Touch(brick, PORT_1)
ultrasonic = Ultrasonic(brick, PORT_2)
compass = Ultrasonic(brick, PORT_3)

# LINE FOLLOW VARIABLES
turningPower = 65  # 70, normalized, motor power used when turning in line follow
negInertiaPower = 70  # 65, normalized, motor power for negative inertia
findLineTimeOut = 0.5  # 0.5, time between switching motor to the opposite direction
negInertiaLengthOnWhite = 0.07  # 0.2, time before braking on negative inertia when originally on white
negInertiaLengthOnBlack = 0.07  # 0.05, time before braking on ngative inertia when originally on black (should be smaller than white to prevent overshooting the line)

# CALIBRATION VARIABLES
calTurningPower = 70  # 70, normalized, motor power used to turn when calibrate
calFirstTurnTime = 0.2  # 0.2, time to turn on first turn
calSecondTurnTme = 0.1  # 0.15, time to turn on second turn
calDelta = 10  # no default since it's new, range of light values for which line follow continues going straight (range is 2 * delta)
Esempio n. 43
0
brick = nxtConnect.btConnect(brickName)

from time import sleep
from nxt.motor import SynchronizedMotors

# see files in library ( /usr/local/lib/python2.7/dist-packages/nxt )
# for a more comprehensive list of ports / commands available
from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Light, Sound, Touch, Ultrasonic
from nxt.sensor import PORT_1, PORT_2, PORT_3, PORT_4

try:
    motorLeft = Motor(brick, PORT_A)  # plug motor into Port A
    motorRight = Motor(brick, PORT_B)  # plug motor into Port B
    motorBoth = nxt.SynchronizedMotors(motorRight, motorLeft, 0)
    ultraSensor = Ultrasonic(brick,
                             PORT_1)  # plug ultrasonic sensor into Port 1

    #motorSync = SynchronizedMotors(motorA, motorB, 10000)
    #motorSync.idle()
    while ultraSensor.get_sample() > 10:
        print("Current ultrasonic sensor state: {}".format(
            ultraSensor.get_sample()))
        motorBoth.run(power=-120)
        '''motorA.run(power = -128) #go forward
        motorB.run(power = -128) #go forward'''

finally:
    Motor(brick, PORT_A).idle()  # otherwise motor keeps running
    Motor(brick, PORT_B).idle()  # otherwise motor keeps running
    print("Terminating Program")
    brick.sock.close()