예제 #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)
예제 #2
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_4)
        # 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()
예제 #3
0
class Strider(object):
    def __init__(self, brick="NXT"):
        r"""Creates a new Strider 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.back_leg = Motor(brick, PORT_B)
        self.left_leg = Motor(brick, PORT_C)
        self.right_leg = Motor(brick, PORT_A)

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

    def walk(self, direction, time=0):
        [back, left, right] = direction.get_directions()
        self.back_leg.run(back * 80)
        self.left_leg.run(left * 80)
        self.right_leg.run(right * 80)
        if time > 0:
            sleep(time)
            self.stop()

    def show_colour(self, colour):
        self.colour.set_light_color(colour)

    def colour_display(self):
        for colour in [Type.COLORRED, Type.COLORGREEN, Type.COLORBLUE]:
            self.show_colour(colour)
            sleep(2)
        self.show_colour(Type.COLORNONE)

    def stop(self):
        self.back_leg.idle()
        self.left_leg.idle()
        self.right_leg.idle()

    def fire_lasers(self):
        for i in range(0, 5):
            self.brick.play_sound_file(False, "! Laser.rso")
예제 #4
0
from time import sleep

# 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

# use try with finally to stop motors at end, even if program
# encountered an (programming) error.
try:
    touchSensor = Touch(brick, PORT_1) # plug touch sensor into Port 1
    motor = Motor(brick, PORT_A) # plug motor into Port A
    
    # Note: |Power| <50 might not be strong enough to turn motor / 
    # overcome the internal friction
    motor.run(power = 70) # go forward
    sleep(2.5) # let NXT do its thing for 2.5 seconds
    motor.run(power = -70) # go backward
    sleep(2)
    
    # will read when this line of code is reached, so KEEP sensor
    # pressed till then
    print("Current touch sensor state: {}".format(
            touchSensor.get_sample()))
    
finally:
    Motor(brick, PORT_A).idle() # otherwise motor keeps running
    print("Terminating Program")
    brick.sock.close()
예제 #5
0
#!/usr/bin/env python

import nxt.bluesock
import time
import ipdb
from nxt.motor import Motor, PORT_A, PORT_B
from nxt.sensor import Ultrasonic, Sound, PORT_2, PORT_4

robo = nxt.bluesock.BlueSock('00:16:53:08:51:40').connect()

direita = Motor(robo, PORT_A)
esquerda = Motor(robo, PORT_B)

direita.run(-82)
esquerda.run(-80)
 
time.sleep(20)

direita.brake()
esquerda.brake()
direita.turn(-40,500)
#ipdb.set_trace()
direita.run(-82)
esquerda.run(-80)
time.sleep(7)
direita.brake()
esquerda.brake()
direita.turn(-40,500)
direita.run(-82)
esquerda.run(-80)
time.sleep(6)
예제 #6
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
예제 #7
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 
예제 #8
0
class NXTLocomotionCommandHandler:
    def __init__(self,
                 proj,
                 shared_data,
                 leftDriveMotor='PORT_B',
                 rightDriveMotor='PORT_C',
                 steeringMotor='none',
                 steeringGearRatio=1.0,
                 leftForward=True,
                 rightForward=True):
        """
        Locomotion Command handler for NXT Mindstorms.
        
        leftDriveMotor (str): The motor that drives the left side (default='PORT_B')
        rightDriveMotor (str): The motor that drives the right side (default='PORT_C')
        steeringMotor (str): The motor that controls steering, if applicable (default='none')
        steeringGearRatio (float): The gear ratio on the steering control (default=1.0)
        leftForward (bool): Whether forward direction is positive power for the left drive motor (default=True)
        rightForward (bool): Whether forward direction is positive power for the right drive motor (default=True)
        """

        self.nxt = shared_data[
            'NXT_INIT_HANDLER']  # shared data is the nxt and its functions in this case
        self.pose = proj.h_instance['pose']  # pose data is useful for travel
        self.actuator = proj.h_instance['actuator']

        # The following creates a tuple of the drive motors based on user input
        # It also derives the left and right motors as well as a steering motor if used
        # There are currently two modes of drive, differential and non-differential (car)
        self.driveMotors = ()
        self.differentialDrive = False
        self.leftForward = leftForward
        self.rightForward = rightForward
        if (leftDriveMotor == 'PORT_A' or rightDriveMotor == 'PORT_A'):
            self.driveMotors += Motor(self.nxt.brick, PORT_A),
            if (leftDriveMotor == 'PORT_A'):
                self.left = Motor(self.nxt.brick, PORT_A)
            else:
                self.right = Motor(self.nxt.brick, PORT_A)
        if (leftDriveMotor == 'PORT_B' or rightDriveMotor == 'PORT_B'):
            self.driveMotors += Motor(self.nxt.brick, PORT_B),
            if (leftDriveMotor == 'PORT_B'):
                self.left = Motor(self.nxt.brick, PORT_B)
            else:
                self.right = Motor(self.nxt.brick, PORT_B)
        if (leftDriveMotor == 'PORT_C' or rightDriveMotor == 'PORT_C'):
            self.driveMotors += Motor(self.nxt.brick, PORT_C),
            if (leftDriveMotor == 'PORT_C'):
                self.left = Motor(self.nxt.brick, PORT_C)
            else:
                self.right = Motor(self.nxt.brick, PORT_C)

        self.steerMotor = None
        self.steeringRatio = 1
        if (steeringMotor == 'none'):
            self.differentialDrive = True
        if (not self.differentialDrive):
            if (steeringMotor == 'PORT_A'):
                self.steerMotor = Motor(self.nxt.brick, PORT_A)
            if (steeringMotor == 'PORT_B'):
                self.steerMotor = Motor(self.nxt.brick, PORT_B)
            if (steeringMotor == 'PORT_C'):
                self.steerMotor = Motor(self.nxt.brick, PORT_C)
            self.tacho = self.getUsefulTacho(self.steerMotor)
            self.steeringRatio = steeringGearRatio  # Global variable fro steering gear ratio

        self.once = True  # start dead reckoning path record only once

    def sendCommand(self, cmd):
        """     
        Send movement command to the NXT
        """
        # general power specifications
        leftPow = BACK
        if self.leftForward: leftPow = FORTH
        rightPow = BACK
        if self.rightForward: rightPow = FORTH

        def forward(sec, power):  #currently unused
            '''allows for forward movement with power and for time'''
            for motor in self.driveMotors:
                motor.run(power)
            sleep(sec)
            for motor in self.driveMotors:
                motor.idle()

        def go(power):
            '''turns the drive motors on with given power'''
            for motor in self.driveMotors:
                motor.run(power)

        def carTurn(sec, direction):  #currently unused
            '''specifies time based steering'''
            self.steerMotor.run(direction)
            forward(sec, leftPow)
            self.steerMotor.idle()

        def left(leftPow, power):
            '''used for differential drive on left turns'''
            self.left.run(power)
            self.right.run(leftPow)

        def right(rightPow, power):
            '''used for differential drive on right turns'''
            self.left.run(rightPow)
            self.right.run(power)

        def idle():
            '''sets all motors to idle state'''
            for motor in self.driveMotors:
                motor.idle()
            if (not self.differentialDrive):
                self.steerMotor.idle()

        def goToDegree(degree):
            """
            Takes a degree measurement (from the tachometer) and carefully aligns the steering motor to that degree
            """
            curDegree = self.getUsefulTacho(
                self.steerMotor)  #currently angled at this degree
            baseDegree = curDegree
            degreeRange = abs(curDegree -
                              degree)  #distance in degrees it has to run
            leftPower = -75.0  #full power for steering
            rightPower = 75.0
            powerRange = rightPower - MIN  #power range for steering, MIN is the minimum power for movement on steering
            while (
                    curDegree > degree + RANGE or curDegree < degree - RANGE
            ):  #checks to see if the current angle is close enough to the desired
                while (curDegree > degree + RANGE):
                    if (abs(curDegree - degree) < 30):
                        leftPower = -MIN  #small angle change necessitates small power
                    elif (abs(curDegree - degree) > degreeRange):
                        leftPower = -75  #large angle change necessitates max power
                    else:
                        leftPower = -(
                            ((abs(curDegree - degree) / degreeRange) *
                             powerRange) + MIN
                        )  #As you get closer to the angle, decrease power to steering motor
                    self.steerMotor.run(leftPower)
                    lastDegree = curDegree
                    curDegree = self.getUsefulTacho(
                        self.steerMotor)  #get new current degree
                    if (lastDegree == curDegree):
                        break  #implies the motor is stuck
                    if (self.v == 0): break  #check for pause...
                self.steerMotor.idle(
                )  #always idle motors before giving power incase opposite direction
                curDegree = self.getUsefulTacho(
                    self.steerMotor)  #recheck current degre
                while (curDegree < degree - RANGE):  #Same as above
                    if (abs(curDegree - degree) < 30): rightPower = MIN
                    elif (abs(curDegree - degree) > degreeRange):
                        rightPower = 75
                    else:
                        rightPower = (
                            ((abs(degree - curDegree) / degreeRange) *
                             powerRange) + MIN)
                    self.steerMotor.run(rightPower)
                    lastDegree = curDegree
                    curDegree = self.getUsefulTacho(self.steerMotor)
                    if (lastDegree == curDegree): break
                    if (self.v == 0): break  # check for pause...
                self.steerMotor.idle()
                curDegree = self.getUsefulTacho(self.steerMotor)
                if (self.v == 0): break  # check for pause...
            self.steerMotor.idle()
            try:
                self.pose.updateSteerAngle(curDegree, baseDegree)
            except:
                pass

        def difDrive():
            """
            Methodology behind a differential drive.  It uses facing direction and needed direction
            """
            stopped = False
            angle = self.angle * 180 / pi
            # if angle > 0 or angle < 0:
            #print 'angle='+str(angle)+' w='+str(self.angle)
            leftPow = -80  #max powers
            if self.leftForward: leftPow = 80
            rightPow = -80
            if self.rightForward: rightPow = 80
            try:
                if (self.v == 0 and not self.actuator.actuatorMotorOn):
                    idle()  #pause...
                    stopped = True
            except:
                if (self.v == 0):
                    idle()
                    stopped = True
            if stopped:
                pass
            elif angle > .5:  #left turning arc
                if (leftPow > 0):
                    arcPower = (
                        (leftPow - LOW) * (90 - abs(angle)) /
                        90) + LOW  # scaled power based on required omega
                else:
                    arcPower = ((leftPow + LOW) * (90 - abs(angle)) / 90) - LOW
                left(leftPow, arcPower)
            elif angle < -.5:  #right tuning arc
                if (rightPow > 0):
                    arcPower = ((rightPow - LOW) *
                                (90 - abs(angle)) / 90) + LOW
                else:
                    arcPower = ((rightPow + LOW) *
                                (90 - abs(angle)) / 90) - LOW
                right(rightPow, arcPower)
            else:
                go(leftPow)  #straight
            stopped = False

        def nonDifDrive():
            """
            Methodology behind a car type drive.  It checks current pose and uses given vx and vy to calculate whether it should be turning or not.
            """
            pose = self.pose.getPose()
            angle = pose[2] * 180 / pi + 90  #Facing Direction
            stopped = False
            #Orient facing direction between -180 and 180
            while (angle > 180):
                angle -= 360
            while (angle < -180):
                angle += 360
            phi = atan2(vy, vx) * 180 / pi  #Direction to POI
            try:
                if (self.v == 0 and not self.actuator.actuatorMotorOn
                    ):  #pause command sent
                    idle()
                    stopped = True
            except:
                if (self.v == 0):
                    idle()
                    stopped = True
            if stopped:
                pass
            elif (phi + 360 - angle <
                  angle - phi):  #quadrant 3 angle and quadrant 2 phi
                #left
                degree = -MAX_ANGLE * self.steeringRatio
                goToDegree(degree)
            elif (angle + 360 - phi <
                  phi - angle):  #quadrant 2 angle and quadrant 3 phi
                #right
                degree = MAX_ANGLE * self.steeringRatio
                goToDegree(degree)
            elif (phi + RANGE < angle):  #right turn to line up
                #right
                degree = MAX_ANGLE * self.steeringRatio
                goToDegree(degree)
            elif (phi - RANGE > angle):  #left turn to line up
                #left
                degree = -MAX_ANGLE * self.steeringRatio
                goToDegree(degree)
            else:  #general straight direction
                #straight
                degree = self.tacho
                goToDegree(degree)
            if (self.v != 0):  #run drive motors
                go(leftPow * .65)

        stopped = False
        pose = self.pose.getPose()  #get pose from vicon
        vx = cmd[0]  #decompose velocity
        vy = cmd[1]
        if self.leftForward:
            theta = pose[2] - (pi / 2
                               )  #get facing angle (account for vicon axes)
        else:
            theta = pose[2] + (pi / 2)
        self.v = cos(theta) * vx + sin(theta) * vy  #magnitude of v
        if self.once:
            try:
                self.pose.setPose()
            except:
                print 'Not setting pose with dead reckoning'
                pass
            self.once = False
        if (self.differentialDrive):  #handles differential drive
            #theta=theta-pi                          #orient angle for reverse direction of travel
            self.angle = atan2(vy, vx) - theta
            #print 'Vx: '+str(vx)+' Vy: '+str(vy)+' theta: '+str(theta)
            while self.angle > pi:
                self.angle -= 2 * pi
            while self.angle < -pi:
                self.angle += 2 * pi
            difDrive()
        else:  #handles car type drive
            nonDifDrive()

    def getUsefulTacho(self, motor):
        """Turns instance data from tachometer in to useful integer"""
        # the tachometer data from the nxt is not useful in current form, this provides usability
        tacho = tuple(
            int(n) for n in str(motor.get_tacho()).strip('()').split(','))
        return tacho[0]
예제 #9
0
class Mindstorm_Robot:
    def __init__(self, brick_name='NXT'):
        sock = find_one_brick(name=brick_name)
    
        brick = self.brick = sock.connect()
        self.arm = 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)
        self.busy = False

        self.arg_d = {"forward": (self.forward, 2),
                      "backward": (self.backward, 2),
                      "left": (self.turn_left, 2),
                      "right": (self.turn_right, 2),
                      #"power": (self.add_arm_power, 1),
                      #"kick": (self.release_arm, 0),
                      "boot": (self.boot, 0),
                      "sing": (self.sing, 0),
                      "talk": (self.talk, 0)}

    def interpret(self, c):
        self.interpret_exception(c)
        #try: return self.interpret_exception(c)
        #except Exception as e: print(e); return "FAIL"

    def interpret_exception(self, c):
        i = c.rest[0]
        func, args = self.arg_d[i]
        func(*map(int, c.rest[1:1+args]))

    def stop(self):
        self.arm.stop()
        for m in self.legs:
            m.stop()

    def forward(self, X, P):
        if not (1 <= X <= 20 and 5 <= P <= 100):
            return "FAIL"
        self.busy = True
        for motor in self.legs:
            motor.run(power=P)
        time.sleep(X/5)
        self.stop()
        self.busy = False
        return "SUCCESS"

    def backward(self, X, P):
        if not (1 <= X <= 20 and 5 <= P <= 100):
            return "FAIL"
        self.busy = True
        for motor in self.legs:
            motor.run(power=-P)
        time.sleep(X/5)
        self.stop()
        self.busy = False
        return "SUCCESS"

    def turn_left(self, X, P):
        if not (1 <= X <= 20 and 5 <= P <= 100):
            return "FAIL"
        self.busy = True
        self.legs[0].run(power=P)
        self.legs[1].run(power=-P)
        time.sleep(X/5)
        self.stop()
        self.busy = False
        return "SUCCESS"

    def turn_right(self, X, P):
        if not (1 <= X <= 20 and 5 <= P <= 100):
            return "FAIL"
        self.busy = True
        self.legs[0].run(power=-P)
        self.legs[1].run(power=P)
        time.sleep(X/5)
        self.stop()
        self.busy = False
        return "SUCCESS"

    def add_arm_power(self, F):
        return self.reel_arm(int(F/2), 20)

    def reel_arm(self, X, P):
        if not (1 <= X <= 20 and 5 <= P <= 100):
            return "FAIL"
        self.busy = True
        self.arm.run(power=-P)
        time.sleep(X/5)
        self.stop()
        self.busy = False
        return "SUCCESS"

    def boot(self):
        self.add_arm_power(20)

    def release_arm(self):
        X = 5
        P = 60
        self.busy = True
        self.arm.run(power=P)
        time.sleep(X/5)
        self.stop()
        self.busy = False
        return "SUCCESS"

    def play(self, note, dur = 500):
        if note:
            self.brick.play_tone_and_wait(note, dur)
        else:
            time.sleep(0.5)

    def sing(self):
        C = 523
        D = 587
        E = 659
        G = 784
        R = None
        #self.busy = True
        for note in [E, D, C, D, E, E, E, R, \
                     D, D, D, R, \
                     E, G, G, R, E, D, C, D, E, E, E, E, D, D, E, D, C]:
            self.play(note)
        #self.busy = False
        return "SUCCESS"

    def talk(self):
        C = 523
        D = 587
        E = 659
        G = 784
        notes = C,D,E,G
        play_these = [choice(notes) for i in range(randint(2, 8))]
        #self.busy = True
        for p in play_these:
            self.play(p, dur = randint(50, 800))
        #self.busy = False
        return "SUCCESS"
예제 #10
0
import nxt
import nxtConnect  # has to be in search path

brickName = "Team60"
useUSB = False

if useUSB:
    brick = nxt.find_one_brick(name=brickName,
                               strict=True,
                               method=nxt.locator.Method(usb=True,
                                                         bluetooth=True))
else:
    # 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

light = Light(brick, PORT_1)
turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
legPosition = Touch(brick, PORT_3)
#ultrasonic = Sonar(brick, PORT_2)

walkingMotor.run(power=120)
예제 #11
0
파일: robot.py 프로젝트: sdp-2011/sdp-6
class Robot(object):

    LEFT_WHEEL = 0x02  # port C
    RIGHT_WHEEL = 0x00  # port A
    KICKER = 0x01  # port B

    DEFAULT_POWER = 80
    TURN_POWER = 0.8

    BUZZER_HZ = 769

    KICK_DISTANCE = 90

    STATE_DISCONNECTED = -1
    STATE_IDLE = 0
    STATE_UP = 1
    STATE_DOWN = 2
    STATE_RIGHT = 3
    STATE_LEFT = 4

    MAX_MOTOR_POWER = 127

    #NAME = "BrickAshley"
    NAME = "BrickAsh"

    def __init__(self, host=None):

        self.power = -1 * self.DEFAULT_POWER
        self.address = host
        self.state = self.STATE_DISCONNECTED

        self.log = logging.getLogger("Robot")

    def connect(self):
        self.log.info("Connecting ...")
        try:
            if self.address == None:
                self.brick = nxt.find_one_brick().connect()
            else:
                self.brick = BlueSock(self.address).connect()
        except nxt.locator.BrickNotFoundError:
            raise RobotNotFoundError
        except Exception as error:
            raise RobotConnectionError(error)

        self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
        self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
        self.kicker = Motor(self.brick, self.KICKER)
        self.log.info("Set up Motors")

        try:
            #self.kicker.turn(100, 100, brake=True)

            self.log.debug(self.__read_motor_state(self.KICKER))

        except Exception as error:
            self.log.error("kicker reset error: " + str(error))

        self.state = self.STATE_IDLE

        self.__get_info()
        self.log.info("Conected to {name}".format(name=self.name))

        self.buzz()

    def disconnect(self):
        try:
            self.brick = None
            #self.get_info_thread.stop()
            gc.collect()
        except:
            self.log.warning("Unsafe disconect")

        self.state = self.STATE_DISCONNECTED

    def get_name(self):
        self.__get_info()
        return self.name

    def set_name(self, name):
        self.brick.set_brick_name(name)
        self.disconnect()
        self.connect()
        self.__get_info()

    def set_power(self, value):
        if value < -1 * self.MAX_MOTOR_POWER or value > self.MAX_MOTOR_POWER:
            raise ValueError("Power can only be +-127")
        else:
            self.power = value
            self.log.info("power set to: " + str(self.power))

    def get_power(self):
        return self.power

    def __get_info(self):
        #self.get_info_thread = threading.Timer(30, self.__get_info)
        #self.get_info_thread.start()
        self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info(
        )
        self.battery = self.brick.get_battery_level()
        self.log.info(
              "Info: \n\tName: {name}" \
              "\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
              "Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
              signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)
              )

    def up(self):
        self.log.debug("go up")
        if self.state != self.STATE_UP:
            self.state = self.STATE_UP
            self.leftWhell.run(power=self.power)
            self.rightWhell.run(power=self.power)

    def down(self):
        self.log.debug("go down")
        if self.state != self.STATE_DOWN:
            self.state = self.STATE_DOWN
            self.leftWhell.run(power=-1 * self.power)
            self.rightWhell.run(power=-1 * self.power)

    def right(self, withBrake=False):
        self.log.debug("go right")
        if self.state != self.STATE_RIGHT:
            self.state = self.STATE_RIGHT
            self.leftWhell.run(power=self.power * self.TURN_POWER)
            if withBrake:
                self.rightWhell.brake()
            else:
                self.rightWhell.run(power=-self.power * self.TURN_POWER)

    def left(self, withBrake=False):
        self.log.debug("go left")
        if self.state != self.STATE_LEFT:
            self.state = self.STATE_LEFT
            if withBrake:
                self.leftWhell.brake()
            else:
                self.leftWhell.run(power=-self.power * self.TURN_POWER)
            self.rightWhell.run(power=self.power * self.TURN_POWER)

    def stop(self):
        self.log.debug("go stop")
        self.state = self.STATE_IDLE
        self.leftWhell.brake()
        self.rightWhell.brake()

    def buzz(self):
        self.log.debug("buzz")
        self.brick.play_tone_and_wait(self.BUZZER_HZ, 1000)

    def kick(self):
        self.log.debug("kick")
        self.kicker.turn(-127, self.KICK_DISTANCE, brake=True)
        threading.Timer(1.5, self.__kick_reset).start()

    def __kick_reset(self):
        self.kicker.turn(127, self.KICK_DISTANCE, brake=True)

    #def __del__(self):
    #  self.log.debug("__del__")
    #  if self.brick != None:
    #    self.disconnect()

    def __read_motor_state(self, port):
        values = self.brick.get_output_state(port)
        self.log.debug("__read_motor_state: values='{0}'".format(values))
        #state, tacho = get_tacho_and_state(values)
        #self.log.debug("__read_motor_state: state='{0}', tacho='{1}'".format(state, tacho))
        left, kick, right = values[-3:]

        if port == self.KICKER:
            return kick
        elif port == self.LEFT_WHEEL:
            return left
        elif port == self.RIGHT_WHEEL:
            return left
        else:
            raise Exception("meh")

    def get_state(self):
        self.__read_motor_state(self.KICKER)
        self.__read_motor_state(self.LEFT_WHEEL)
        self.__read_motor_state(self.RIGHT_WHEEL)

    def kick_to(self, angle, kpower=127, withBrake=True):
        state, tacho = self.__read_motor_state(self.KICKER)
        if angle < tacho:
            self.kicker.turn(-kpower, tacho - angle, brake=withBrake)
        else:
            self.kicker.turn(+kpower, angle - tacho, brake=withBrake)
예제 #12
0
파일: robot.py 프로젝트: TheBiggerGuy/sdp6
class Robot(object):
  
  LEFT_WHEEL  = 0x02 # port C
  RIGHT_WHEEL = 0x00 # port A
  KICKER      = 0x01 # port B
    
  DEFAULT_POWER = 80
  TURN_POWER    = 0.8
  
  BUZZER = 769
  
  #NAME = "BrickAshley"
  NAME = "BrickAsh"
    
  def __init__(self, host=None):
    
    self.power = self.DEFAULT_POWER
    self.address = host   
    
    self.log = logging.getLogger("Robot")
    
    self.connect()
    
    self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
    self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
    self.kicker = Motor(self.brick, self.KICKER)
    self.log.info("Set up Motors")
    
    try:
      #self.kicker.turn(100, 100, brake=True)
      
      self.log.debug(self.__read_motor_state(self.KICKER))
      
    except Exception as error:
      self.log.error("kicker reset error: " + str(error))
  
  def connect(self):
    self.log.info("Connecting ...")
    try:
      if self.address == None:
        self.brick = nxt.find_one_brick().connect()
      else:
        self.brick = BlueSock(self.address).connect()      
    except nxt.locator.BrickNotFoundError:
      raise RobotNotFoundError
    except Exception as error:
      raise RobotConnectionError(error)
    
    self.__get_info()
    self.log.info("Conected to {name}".format(name=self.name))
  
  def disconnect(self):
    try:
      self.brick = None
      #self.get_info_thread.stop()
      gc.collect()
    except:
      self.log.warning("Unsafe disconect")
      pass
  
  def get_name(self):
    self.__get_info()
    return self.name
  
  def set_name(self, name):
    self.brick.set_brick_name(name)
    self.disconnect()
    self.connect()
    self.__get_info()
  
  def set_power(self, value):
    value=int(value)
    if value < -127 or value > 127:
      raise ValueError("Power can only be +-127")
    self.power = value
  
  def get_power(self):
    return self.power
  
  def __get_info(self):
    #self.get_info_thread = threading.Timer(30, self.__get_info)
    #self.get_info_thread.start()
    self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info()
    self.battery = self.brick.get_battery_level()
    self.log.info(
          "Info: \n\tName: {name}" \
          "\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
          "Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
          signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)
          )
  
  def up(self):
    self.log.debug("go up")
    self.leftWhell.run(power=self.power)
    self.rightWhell.run(power=self.power)
  
  def down(self):
    self.log.debug("go down")
    self.brick.play_tone_and_wait(self.BUZZER, 1000)
    self.leftWhell.run(power=-self.power)
    self.rightWhell.run(power=-self.power)
  
  def right(self, withBrake=False):
    self.log.debug("go right")
    self.leftWhell.run(power=self.power*self.TURN_POWER)
    if withBrake:
      self.rightWhell.brake()
    else:
      self.rightWhell.run(power=-self.power*self.TURN_POWER)
  
  def left(self, withBrake=False):
    self.log.debug("go left")
    if withBrake:
      self.leftWhell.brake()
    else:
      self.leftWhell.run(power=-self.power*self.TURN_POWER)
    self.rightWhell.run(power=self.power*self.TURN_POWER)
  
  def stop(self):
    self.log.debug("go stop")
    self.leftWhell.brake()
    self.rightWhell.brake()
    #self.kicker.brake()
  
  def buzz(self):
    self.log.debug("buzz")
    self.brick.play_tone_and_wait(self.BUZZER, 1000)
  
  def kick(self):
    self.log.debug("kick")
    self.kicker.turn(-127, 85, brake=True)
    threading.Timer(1.5, self.__kick_reset).start()
  
  def __kick_reset(self):
    self.kicker.turn(127, 90, brake=False)
  
  #def __del__(self):
  #  self.log.debug("__del__")
  #  if self.brick != None:
  #    self.disconnect()
  
  def __read_motor_state(self, port):
    values = self.brick.get_output_state(port)
    self.log.debug("__read_motor_state: values='{0}'".format(values))
    #state, tacho = get_tacho_and_state(values)
    #self.log.debug("__read_motor_state: state='{0}', tacho='{1}'".format(state, tacho))
    left, kick, right = values[-3:]
    
    if port == self.KICKER:
      return kick
    elif port == self.LEFT_WHEEL:
      return left
    elif port == self.RIGHT_WHEEL:
      return left
    else:
      raise Exception("meh")
  
  def get_state(self):
    self.__read_motor_state(self.KICKER)
    self.__read_motor_state(self.LEFT_WHEEL)
    self.__read_motor_state(self.RIGHT_WHEEL)
  
  def kick_to(self, angle, kpower=127, withBrake=True):
    state, tacho = self.__read_motor_state(self.KICKER)
    if angle < tacho:
      self.kicker.turn(-kpower, tacho-angle, brake=withBrake)
    else:
      self.kicker.turn(+kpower, angle-tacho, brake=withBrake)
예제 #13
0
useUSB = False

if useUSB:
    brick = nxt.find_one_brick(
        name = brickName,
        strict = True,
        method = nxt.locator.Method(usb = True, bluetooth = True))
else:
    # 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

#######################################################################
## Then, you can specify what you want the NXT to do
#######################################################################

from time import sleep

from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4

motorLeft = Motor(brick, PORT_B)
motorRight = Motor(brick, PORT_C)
touch = Touch(brick, PORT_4)

motorLeft.run(power = -70)
    
motorRight.run(power = -120)
예제 #14
0
#######################################################################
## Then, you can specify what you want the NXT to do
#######################################################################

from time import sleep

from nxt.motor import Motor, PORT_A, PORT_B, PORT_C
from nxt.sensor import Touch, PORT_4, PORT_3

turningMotor = Motor(brick, PORT_B)
walkingMotor = Motor(brick, PORT_C)
turnerSwitch = Touch(brick, PORT_4)
legPosition = Touch(brick, PORT_3)

while True:
    if turnerSwitch.is_pressed() == False:
        turningMotor.run(power=0)
        a = 0
        walkingMotor.run(power=120)

    else:
        if legPosition.is_pressed() == False:
            walkingMotor.run(power=100)
        else:
            a = 1
        if a == 1:
            walkingMotor.run(power=0)
            walkingMotor.brake()
            turningMotor.run(power=120)
예제 #15
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()
예제 #16
0
class NXTLocomotionCommandHandler:
    def __init__(self, proj, shared_data, leftDriveMotor='PORT_B', rightDriveMotor='PORT_C', steeringMotor='none', steeringGearRatio=1.0, leftForward=True, rightForward=True):
        """
        Locomotion Command handler for NXT Mindstorms.
        
        leftDriveMotor (str): The motor that drives the left side (default='PORT_B')
        rightDriveMotor (str): The motor that drives the right side (default='PORT_C')
        steeringMotor (str): The motor that controls steering, if applicable (default='none')
        steeringGearRatio (float): The gear ratio on the steering control (default=1.0)
        leftForward (bool): Whether forward direction is positive power for the left drive motor (default=True)
        rightForward (bool): Whether forward direction is positive power for the right drive motor (default=True)
        """
            
        self.nxt = shared_data['NXT_INIT_HANDLER'] # shared data is the nxt and its functions in this case
        self.pose = proj.h_instance['pose'] # pose data is useful for travel
        self.actuator = proj.h_instance['actuator']
        
        # The following creates a tuple of the drive motors based on user input
        # It also derives the left and right motors as well as a steering motor if used
        # There are currently two modes of drive, differential and non-differential (car)
        self.driveMotors=()
        self.differentialDrive = False
        self.leftForward = leftForward
        self.rightForward = rightForward
        if(leftDriveMotor=='PORT_A' or rightDriveMotor=='PORT_A'): 
            self.driveMotors+=Motor(self.nxt.brick, PORT_A),
            if(leftDriveMotor=='PORT_A'):
                self.left=Motor(self.nxt.brick, PORT_A)
            else:
                self.right=Motor(self.nxt.brick, PORT_A)
        if(leftDriveMotor=='PORT_B' or rightDriveMotor=='PORT_B'): 
            self.driveMotors+=Motor(self.nxt.brick, PORT_B),
            if(leftDriveMotor=='PORT_B'):
                self.left=Motor(self.nxt.brick, PORT_B)
            else:
                self.right=Motor(self.nxt.brick, PORT_B)
        if(leftDriveMotor=='PORT_C' or rightDriveMotor=='PORT_C'): 
            self.driveMotors+=Motor(self.nxt.brick, PORT_C),
            if(leftDriveMotor=='PORT_C'):
                self.left=Motor(self.nxt.brick, PORT_C)
            else:
                self.right=Motor(self.nxt.brick, PORT_C)
        
        self.steerMotor=None
        self.steeringRatio=1
        if(steeringMotor=='none'): 
            self.differentialDrive=True
        if(not self.differentialDrive):
            if(steeringMotor=='PORT_A'): self.steerMotor = Motor(self.nxt.brick, PORT_A)
            if(steeringMotor=='PORT_B'): self.steerMotor = Motor(self.nxt.brick, PORT_B)
            if(steeringMotor=='PORT_C'): self.steerMotor = Motor(self.nxt.brick, PORT_C)
            self.tacho = self.getUsefulTacho(self.steerMotor)
            self.steeringRatio=steeringGearRatio # Global variable fro steering gear ratio
        
        self.once=True # start dead reckoning path record only once    
        
    def sendCommand(self, cmd):
        """     
        Send movement command to the NXT
        """
        # general power specifications
        leftPow = BACK
        if self.leftForward: leftPow = FORTH
        rightPow = BACK
        if self.rightForward: rightPow = FORTH
      
        def forward(sec, power): #currently unused
            '''allows for forward movement with power and for time'''
            for motor in self.driveMotors:
                motor.run(power)
            sleep(sec)
            for motor in self.driveMotors:
                motor.idle()
                
        def go(power):
            '''turns the drive motors on with given power'''
            for motor in self.driveMotors:
                motor.run(power)
        
        def carTurn(sec, direction): #currently unused 
            '''specifies time based steering'''
            self.steerMotor.run(direction)
            forward(sec,leftPow)            
            self.steerMotor.idle()
        
        def left(leftPow, power): 
            '''used for differential drive on left turns'''
            self.left.run(power)
            self.right.run(leftPow)
            
        def right(rightPow, power):
            '''used for differential drive on right turns'''
            self.left.run(rightPow)
            self.right.run(power)
            
        def idle():
            '''sets all motors to idle state'''
            for motor in self.driveMotors:
                motor.idle()
            if(not self.differentialDrive): 
                self.steerMotor.idle()
        
        def goToDegree(degree):
            """
            Takes a degree measurement (from the tachometer) and carefully aligns the steering motor to that degree
            """
            curDegree = self.getUsefulTacho(self.steerMotor)            #currently angled at this degree
            baseDegree=curDegree
            degreeRange = abs(curDegree-degree)                         #distance in degrees it has to run
            leftPower = -75.0                                           #full power for steering
            rightPower = 75.0
            powerRange=rightPower-MIN                                   #power range for steering, MIN is the minimum power for movement on steering
            while(curDegree>degree+RANGE or curDegree<degree-RANGE):    #checks to see if the current angle is close enough to the desired
                while(curDegree>degree+RANGE):
                    if(abs(curDegree-degree)<30): leftPower=-MIN        #small angle change necessitates small power
                    elif(abs(curDegree-degree)>degreeRange): leftPower = -75    #large angle change necessitates max power
                    else: leftPower = -(((abs(curDegree-degree)/degreeRange)*powerRange)+MIN)   #As you get closer to the angle, decrease power to steering motor
                    self.steerMotor.run(leftPower)
                    lastDegree=curDegree
                    curDegree=self.getUsefulTacho(self.steerMotor)      #get new current degree
                    if(lastDegree==curDegree): break                    #implies the motor is stuck
                    if(self.v==0): break                                #check for pause...
                self.steerMotor.idle()                                  #always idle motors before giving power incase opposite direction
                curDegree=self.getUsefulTacho(self.steerMotor)          #recheck current degre
                while(curDegree<degree-RANGE):                          #Same as above
                    if(abs(curDegree-degree)<30): rightPower=MIN
                    elif(abs(curDegree-degree)>degreeRange): rightPower = 75
                    else: rightPower = (((abs(degree-curDegree)/degreeRange)*powerRange)+MIN)
                    self.steerMotor.run(rightPower)
                    lastDegree=curDegree
                    curDegree=self.getUsefulTacho(self.steerMotor)
                    if(lastDegree==curDegree): break
                    if(self.v==0): break # check for pause...
                self.steerMotor.idle()
                curDegree=self.getUsefulTacho(self.steerMotor)
                if(self.v==0): break # check for pause...
            self.steerMotor.idle()
            try:
                self.pose.updateSteerAngle(curDegree,baseDegree)
            except:
                pass
    
        def difDrive():
            """
            Methodology behind a differential drive.  It uses facing direction and needed direction
            """
            stopped=False
            angle = self.angle*180/pi
           # if angle > 0 or angle < 0:
                #print 'angle='+str(angle)+' w='+str(self.angle)
            leftPow = -80                           #max powers
            if self.leftForward: leftPow = 80
            rightPow = -80
            if self.rightForward: rightPow = 80
            try:
                if(self.v==0 and not self.actuator.actuatorMotorOn):
                    idle()                              #pause...
                    stopped=True
            except:
                if(self.v==0):
                    idle()
                    stopped=True
            if stopped:
                pass
            elif angle>.5:                           #left turning arc
                if(leftPow>0): arcPower=((leftPow-LOW)*(90-abs(angle))/90)+LOW # scaled power based on required omega
                else: arcPower=((leftPow+LOW)*(90-abs(angle))/90)-LOW
                left(leftPow,arcPower)
            elif angle<-.5:                           #right tuning arc
                if(rightPow>0): arcPower=((rightPow-LOW)*(90-abs(angle))/90)+LOW
                else: arcPower=((rightPow+LOW)*(90-abs(angle))/90)-LOW
                right(rightPow,arcPower)                
            else:
                go(leftPow)                         #straight
            stopped=False
        
        def nonDifDrive():
            """
            Methodology behind a car type drive.  It checks current pose and uses given vx and vy to calculate whether it should be turning or not.
            """
            pose = self.pose.getPose() 
            angle = pose[2]*180/pi+90               #Facing Direction
            stopped=False
            #Orient facing direction between -180 and 180
            while(angle>180): angle-=360 
            while(angle<-180): angle+=360
            phi = atan2(vy,vx)*180/pi               #Direction to POI
            try:
                if(self.v==0 and not self.actuator.actuatorMotorOn):  #pause command sent
                    idle()
                    stopped=True
            except:
                if(self.v==0):
                    idle()
                    stopped=True
            if stopped:
                pass
            elif(phi+360-angle<angle-phi):          #quadrant 3 angle and quadrant 2 phi
                #left
                degree=-MAX_ANGLE*self.steeringRatio
                goToDegree(degree)
            elif(angle+360-phi<phi-angle):          #quadrant 2 angle and quadrant 3 phi
                #right
                degree=MAX_ANGLE*self.steeringRatio
                goToDegree(degree)
            elif(phi+RANGE<angle):                  #right turn to line up
                #right
                degree=MAX_ANGLE*self.steeringRatio
                goToDegree(degree)
            elif(phi-RANGE>angle):                  #left turn to line up
                #left
                degree=-MAX_ANGLE*self.steeringRatio
                goToDegree(degree)
            else:                                   #general straight direction
                #straight
                degree=self.tacho
                goToDegree(degree)
            if(self.v!=0):                          #run drive motors
                go(leftPow*.65)
        
        stopped=False
        pose = self.pose.getPose()                  #get pose from vicon
        vx=cmd[0]                                   #decompose velocity
        vy=cmd[1]
        if self.leftForward:
            theta = pose[2]-(pi/2)  #get facing angle (account for vicon axes)
        else:
            theta = pose[2]+(pi/2) 
        self.v = cos(theta)*vx+sin(theta)*vy        #magnitude of v
        if self.once:
            try:
                self.pose.setPose()
            except:
                print 'Not setting pose with dead reckoning'
                pass
            self.once=False
        if(self.differentialDrive):                 #handles differential drive
            #theta=theta-pi                          #orient angle for reverse direction of travel
            self.angle = atan2(vy,vx) - theta
            #print 'Vx: '+str(vx)+' Vy: '+str(vy)+' theta: '+str(theta)
            while self.angle>pi: self.angle-=2*pi
            while self.angle<-pi: self.angle+=2*pi
            difDrive()
        else:                                       #handles car type drive
            nonDifDrive()
            
    def getUsefulTacho(self, motor):
        """Turns instance data from tachometer in to useful integer"""
        # the tachometer data from the nxt is not useful in current form, this provides usability
        tacho = tuple(int(n) for n in str(motor.get_tacho()).strip('()').split(','))
        return tacho[0]
            
예제 #17
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()
예제 #18
0
class Robot(object):

    LEFT_WHEEL = 0x02  # port C
    RIGHT_WHEEL = 0x00  # port A
    KICKER = 0x01  # port B

    DEFAULT_POWER = 80
    TURN_POWER = 0.8

    BUZZER = 769

    #NAME = "BrickAshley"
    NAME = "BrickAsh"

    def __init__(self, host=None):

        self.power = self.DEFAULT_POWER
        self.address = host

        self.connect()

        self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
        self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
        self.kicker = Motor(self.brick, self.KICKER)
        print "Set up Motors"

        try:
            self.kicker.turn(100, 100, brake=True)
        except Exception as error:
            print error

    def connect(self):
        print "Connecting ..."
        try:
            if self.address == None:
                self.brick = nxt.find_one_brick().connect()
            else:
                self.brick = BlueSock(self.address).connect()
        except nxt.locator.BrickNotFoundError:
            raise RobotNotFoundError
        except BluetoothError as error:
            raise RobotConnectionError(error)

        self.__get_info()
        print "Conected to {name}".format(name=self.name)

    def disconnect(self):
        try:
            self.brick = None
            gc.collect()
        except:
            print "Unsafe disconect"

    def get_name(self):
        self.__get_info()
        return self.name

    def set_name(self, name):
        self.brick.set_brick_name(name)
        self.disconnect()
        self.connect()
        self.__get_info()

    def set_power(self, value):
        value = int(value)
        if value < -128 or value > 128:
            pass
            # TODO
        self.power = value

    def get_power(self):
        return self.power

    def __get_info(self):
        threading.Timer(30, self.__get_info).start()
        self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info(
        )
        self.battery = self.brick.get_battery_level()
        print "Info: \n\tName: {name}" \
              "\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
              "Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
              signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)

    def up(self):
        print "go up"
        self.leftWhell.run(power=self.power)
        self.rightWhell.run(power=self.power)

    def down(self):
        print "go down"
        self.brick.play_tone_and_wait(self.BUZZER, 1000)
        self.leftWhell.run(power=-self.power)
        self.rightWhell.run(power=-self.power)

    def right(self, withBrake=False):
        print "go right"
        self.leftWhell.run(power=self.power * self.TURN_POWER)
        if withBrake:
            self.rightWhell.brake()
        else:
            self.rightWhell.run(power=-self.power * self.TURN_POWER)

    def left(self, withBrake=False):
        print "go left"
        if withBrake:
            self.leftWhell.brake()
        else:
            self.leftWhell.run(power=-self.power * self.TURN_POWER)
        self.rightWhell.run(power=self.power * self.TURN_POWER)

    def stop(self):
        print "go stop"
        self.leftWhell.brake()
        self.rightWhell.brake()
        self.kicker.brake()

    def buzz(self):
        print "buzz"
        self.brick.play_tone_and_wait(self.BUZZER, 1000)

    def kick(self):
        print "kick"
        self.kicker.turn(-127, 85, brake=True)
        sleep(1.5)
        self.kicker.turn(127, 90, brake=True)
예제 #19
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)
예제 #20
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)
예제 #21
0
def motor_start(letter, power):
    m = Motor(b, MOTORS[letter.upper()])
    m.run(power=int(power))
    return 'OK'
예제 #22
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()
예제 #23
0
motor.turn(30, 50, brake=True, timeout=1.5, emulate=True)
sleep(0.05)
print(motor.get_tacho())
motor.brake()
white = light.get_lightness()
print("white = ", white)
motor.turn(-30, 50, brake=True, timeout=1.5, emulate=True)
threshold = (black + white) / 2
print(motor.get_tacho())
print("Threshold = ", threshold)

while light.get_lightness() < 1020:
    light.get_lightness()
    if light.get_lightness() > threshold:
        light.get_lightness()
        motor.run(power=10)
        light.get_lightness()

    if light.get_lightness() < threshold:
        light.get_lightness()
        motor.run(power=-10)
        light.get_lightness()

    print("light = ", light.get_lightness())
    angle = motor.get_tacho()
    print(angle)
motor.idle()
'''
lightSensor = Light(brick, PORT_1)
gyro = Gyro(brick, PORT_2)
예제 #24
0
파일: robot.py 프로젝트: TheBiggerGuy/sdp6
class Robot(object):
  
  LEFT_WHEEL  = 0x02 # port C
  RIGHT_WHEEL = 0x00 # port A
  KICKER      = 0x01 # port B
    
  DEFAULT_POWER = 80
  TURN_POWER    = 0.8
  
  BUZZER = 769
  
  #NAME = "BrickAshley"
  NAME = "BrickAsh"
    
  def __init__(self, host=None):
    
    self.power = self.DEFAULT_POWER
    self.address = host   
    
    self.connect()
    
    self.leftWhell = Motor(self.brick, self.LEFT_WHEEL)
    self.rightWhell = Motor(self.brick, self.RIGHT_WHEEL)
    self.kicker = Motor(self.brick, self.KICKER)
    print "Set up Motors"
    
    try:
      self.kicker.turn(100, 100, brake=True)
    except Exception as error:
      print error
  
  def connect(self):
    print "Connecting ..."
    try:
      if self.address == None:
        self.brick = nxt.find_one_brick().connect()
      else:
        self.brick = BlueSock(self.address).connect()      
    except nxt.locator.BrickNotFoundError:
      raise RobotNotFoundError
    except BluetoothError as error:
      raise RobotConnectionError(error)
    
    self.__get_info()
    print "Conected to {name}".format(name=self.name)
  
  def disconnect(self):
    try:
      self.brick = None
      gc.collect()
    except:
      print "Unsafe disconect"
      
  def get_name(self):
    self.__get_info()
    return self.name
  
  def set_name(self, name):
    self.brick.set_brick_name(name)
    self.disconnect()
    self.connect()
    self.__get_info()
  
  def set_power(self, value):
    value=int(value)
    if value < -128 or value > 128:
      pass
      # TODO
    self.power = value
  
  def get_power(self):
    return self.power
  
  def __get_info(self):
    threading.Timer(30, self.__get_info).start()
    self.name, self.host, self.signal_strength, self.user_flash = self.brick.get_device_info()
    self.battery = self.brick.get_battery_level()
    print "Info: \n\tName: {name}" \
          "\n\tBT MAC: {host}\n\tBT signal: {signal}\n\t" \
          "Memory: {memory}\n\tBattery: {voltage}mV".format(name=self.name, host=self.host, \
          signal=self.signal_strength, memory=self.user_flash, voltage=self.battery)
  
  def up(self):
    print "go up"
    self.leftWhell.run(power=self.power)
    self.rightWhell.run(power=self.power)
  
  def down(self):
    print "go down"
    self.brick.play_tone_and_wait(self.BUZZER, 1000)
    self.leftWhell.run(power=-self.power)
    self.rightWhell.run(power=-self.power)
  
  def right(self, withBrake=False):
    print "go right"
    self.leftWhell.run(power=self.power*self.TURN_POWER)
    if withBrake:
      self.rightWhell.brake()
    else:
      self.rightWhell.run(power=-self.power*self.TURN_POWER)
  
  def left(self, withBrake=False):
    print "go left"
    if withBrake:
      self.leftWhell.brake()
    else:
      self.leftWhell.run(power=-self.power*self.TURN_POWER)
    self.rightWhell.run(power=self.power*self.TURN_POWER)
  
  def stop(self):
    print "go stop"
    self.leftWhell.brake()
    self.rightWhell.brake()
    self.kicker.brake()
  
  def buzz(self):
    print "buzz"
    self.brick.play_tone_and_wait(self.BUZZER, 1000)
  
  def kick(self):
    print "kick"
    self.kicker.turn(-127, 85, brake=True)
    sleep(1.5)
    self.kicker.turn(127, 90, brake=True)
예제 #25
0
pygame.joystick.init()
pygame.display.init()

jcount = pygame.joystick.get_count()
if jcount == 0 : raise 'No joystick detected'

for i in range(jcount) :
    js = pygame.joystick.Joystick(i)
    js.init()

while True:  
    ev = pygame.event.wait()
    if ev.type == pygame.JOYAXISMOTION and ev.joy == 0 :
        if ev.axis == 0 :
            yaw.run(ev.value * 10)
        elif ev.axis == 2 :
            pitch.run(ev.value * -5)
    elif ev.type == pygame.JOYBUTTONDOWN :
        pass
    elif ev.type == pygame.JOYBUTTONUP :
        if ev.button == 4 :
            yaw.stop()
            yaw.run(0)
            
            pitch.stop()
            pitch.run(0)
            
            sock.close()
            sys.exit(0)