Beispiel #1
0
def test_regul():
    """
    Make a square with the robot and at the end make it turn the
    other direction  (just for fun).
    """
    DISTANCE = 50
    ANGLE = 90

    motors = Motors(5)
    motors.forward(DISTANCE)
    wait_motors(motors)
    motors.turn_left(ANGLE)
    wait_motors(motors)
    motors.forward(DISTANCE)
    wait_motors(motors)
    motors.turn_left(ANGLE)
    wait_motors(motors)
    motors.forward(DISTANCE)
    wait_motors(motors)
    motors.turn_left(ANGLE)
    wait_motors(motors)
    motors.forward(DISTANCE)
    wait_motors(motors)
    # We can't turn more than 255 degrees because
    # the data send to the Arduino is a 8bits value.
    motors.turn_right(180)
    wait_motors(motors)
    motors.turn_right(90)
    wait_motors(motors)
Beispiel #2
0
    def __init__(self, verbose=False):
        self.verbose = verbose
        self.status_counter = 0
        self.state = self.STATE_WAITING
        self.currenttime = 0
        self.flex_fish_limit = self.FLEX_FISH_LIMIT
        self.player = Player(self, verbose)
        self.adc_sensors = AdcSensors(self, verbose)
        self.motors = Motors(self, verbose)
        self.web_connection = WebConnection(self, verbose)
        self.gps_tracker = GpsTracker(self, verbose)
        self.video = Video(self, verbose)

        # Initial valus for settings
        # Speed: (0-1). 1 = Full speed ahead
        # Turn = -1 - +1  = +1 Only left motor on (turn to right)
        #                    0 Both motors same speed
        #                   -1 Only right motor on (turn to left)
        self.speed = 0.0
        self.turn = 0.0
        # speed style examples:
        #   - Constant speed = (low_speed_percent = 100)
        #   - Stop and go jigging with 6 sec motor on and 4 sec stop. low_speed_percent = 0,speed_change_cycle = 10, speed_motors_full_percent = 60
        #   - Trolling with 10 sec half speed and 5 sec full speed. low_speed_percent = 50, speed_change_cycle = 15, speed_motors_full_percent = 66.66
        self.speed_change_cycle = 0
        self.speed_motors_full_percent = 100
        self.low_speed_percent = 0

        # Play music or not
        self.play_music = False
Beispiel #3
0
def lineTest():
    reflect = ReflectanceSensors()
    ZumoButton().wait_for_press()
    motor = Motors()

    stuck = CheckStuck()

    camera = Camera()

    motob = Motob(motor)
    arbitrator = Arbitrator(motob=motob)

    sensob = Sensob()
    sensob.set_sensors([reflect])
    bbcon = BBCON(arbitrator=arbitrator, motob=motob)
    bbcon.add_sensob(sensob)
    bbcon.set_checkStucker(stuck)

    b = LineFollow(1, [sensob])
    bbcon.add_behavior(b)


    timesteps = 0
    while timesteps < 30:
        bbcon.run_one_timestep()
        timesteps += 1
Beispiel #4
0
def lineCollision():
    reflect = ReflectanceSensors()
    ZumoButton().wait_for_press()
    motor = Motors()
    stuck = CheckStuck()
    ultra = Ultrasonic()
    proxim = IRProximitySensor()
    sensobCol = Sensob()
    sensobCol.set_sensors([ultra, proxim])
    motob = Motob(motor=motor)
    sensobLine = Sensob()
    sensobLine.set_sensors([reflect])

    arb = Arbitrator(motob=motob)
    bbcon = BBCON(arbitrator=arb, motob=motob)
    bbcon.set_checkStucker(stuck)
    line = LineFollow(1, [sensobLine])
    col = CollisionAvoidance(1, [sensobCol])
    bbcon.add_behavior(line)
    bbcon.add_behavior(col)
    bbcon.add_sensob(sensobCol)
    bbcon.add_sensob(sensobLine)

    count = 0
    while count < 20:
        bbcon.run_one_timestep()
        count += 1
Beispiel #5
0
def start():
    bbcon = BBCON()
    arb = Arbitrator(bbcon)
    motor = Motors()
    reflect_sens = ReflectanceSensors(False)
    cam = Camera()
    ir = IR()
    ultra = Ultrasonic()

    bbcon.add_motobs(motor)
    bbcon.set_arb(arb)
    bbcon.add_behaviour(AvoidObj(bbcon, ultra, ir))
    bbcon.add_behaviour(Behaviour_line_follower(bbcon, reflect_sens))
    bbcon.add_behaviour(fub(bb=bbcon))
    #behaviour avoid blue, has to be added last, do not change this, will screw up bbcon code
    bbcon.add_behaviour(Behaviour_avoid_blue(bb=bbcon, cam=cam, ultra=ultra))
    bbcon.add_sensob(reflect_sens)
    bbcon.add_sensob(ir)
    bbcon.add_sensob(ultra)
    #cam has to be added last, will screw up bbcon code if not
    bbcon.add_sensob(cam)

    butt = ZumoButton()
    butt.wait_for_press()
    print("Start zumo")

    while True:
        bbcon.run_one_timestep()
Beispiel #6
0
def test1():
    ZumoButton().wait_for_press()
    m = Motors()
    m.turn_right(90)
    m.turn_left(180)
    m.turn_right(360)
    m.turn_left(360)
Beispiel #7
0
    def __init__(self):
        self.motors = Motors()
        #self.marg = MARG()
        # not sure if I want this here, but will add for now
        self.server = Server()

        self.prev_msg = b''
Beispiel #8
0
    def __init__(self):

        self.ub.Init()
        self.tb.Init()
        self.t1 = datetime.now()

        self.tickSpeed = 0.05
        self.us = Ultrasonics(self.ub)
        self.motors = Motors(self.tb, self.ub, self.tickSpeed)
        self.steering = Steering(self.tb, self.ub, self.tickSpeed)

        self.teleLogger = Telemetry("telemetry", "csv").get()
        self.ptc = PanTiltController(self.ub, 270, 135)
        self.joystick_controller = JoystickController(self.tb, self.ub)

        battMin, battMax = self.tb.GetBatteryMonitoringLimits()
        battCurrent = self.tb.GetBatteryReading()
        print('Battery monitoring settings:')
        print('    Minimum  (red)     %02.2f V' % (battMin))
        print('    Half-way (yellow)  %02.2f V' % ((battMin + battMax) / 2))
        print('    Maximum  (green)   %02.2f V' % (battMax))
        print()
        print('    Current voltage    %02.2f V' % (battCurrent))
        print('    Mode %s' % (self.mode))
        print()
Beispiel #9
0
    def __init__(self):
        # Initialize arbitrator
        self.arbitrator = Arbitrator()

        # Initialize motobs (single object for both motors on the Zumo)
        self.motobs.append(Motob(Motors()))

        # Initialize sensors

        self.sensors = {
            'ultrasonic': Ultrasonic(0.05),
            'IR': IRProximitySensor(),
            'reflectance': ReflectanceSensors(False, 0, 900),
            'camera': Camera(),
        }

        self.active_sensors = [self.sensors['ultrasonic'], self.sensors['IR'], self.sensors['reflectance']]


        # Initialize sensobs

        self.sensobs = {
            'distance': DistanceSensob([self.sensors['ultrasonic']]),
            'line_pos': LinePosSensob([self.sensors['reflectance']]),
            'proximity': ProximitySensob([self.sensors['IR']]),
            'red_search': RedSearchSensob([self.sensors['camera']]),
        }

        self.active_sensobs = [self.sensobs['distance'], self.sensobs['line_pos'], self.sensobs['proximity']]

        time.sleep(1)
Beispiel #10
0
def crashTest():
    ZumoButton().wait_for_press()

    sensor = Crash_sensor()

    motor = Motors()

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

    while True:

        counter += 1
        if counter >= 5:

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

        if ir_command == "LEFT":

            motor.left(0.5, 0.1)

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

        if f_value == 1000:
            motor.stop()
            break
Beispiel #11
0
    def __init__(self):

        self.arbit = Arbitrary()
        self.value = None
        self.motorList = []

        self.motorList.append(Motors())
def vsvingsakte():
    speed = 0.5
    ZumoButton().wait_for_press()
    motors = Motors()
    motors.set_value((speed / 2, speed / 2), 1)
    motors.set_value((speed, -speed), 0.1)
    motors.set_value((speed / 2, speed / 2), 1)
def vsvingrask():
    speed = 0.5
    ZumoButton().wait_for_press()
    motors = Motors()
    motors.set_value((speed / 2, speed / 2), 1)
    motors.set_value((0.3, -0.3), 1)
    motors.set_value((speed / 2, speed / 2), 1)
Beispiel #14
0
def trackTest():
    ZumoButton().wait_for_press()
    motor = Motors()
    ultra = Ultrasonic()
    camera = Camera()


    stuck = CheckStuck()
    motob = Motob(motor)
    arbitrator = Arbitrator(motob=motob)

    sensob = Sensob()
    sensob.set_sensors([ultra, camera])

    bbcon = BBCON(arbitrator=arbitrator, motob=motob)
    b = TrackObject(priority=1, sensobs=[sensob])
    bbcon.set_checkStucker(stuck)
    bbcon.add_behavior(b)

    bbcon.activate_behavior(0)
    bbcon.add_sensob(sensob)

    timesteps = 0
    while timesteps < 25:

        bbcon.run_one_timestep()
        timesteps += 1
Beispiel #15
0
def shoot_panorama(camera=Camera(),motors=Motors(),shots=5):
    s = 1
    im = IMR.Imager(image=camera.update()).scale(s,s)
    rotation_time = 3/shots # At a speed of 0.5(of max), it takes about 3 seconds to rotate 360 degrees
    for i in range(shots-1):
        motors.right(0.5,rotation_time)
        im = im.concat_horiz(IMR.Imager(image=camera.update()))
    return im
Beispiel #16
0
def dancer():
    m = Motors()
    m.forward(0.2, 3)
    m.backward(0.2, 3)
    m.right(0.5, 3)
    m.left(0.5, 3)
    m.backward(0.3, 2.5)
    m.set_value([0.5, 0.1], 10)
    m.set_value([-0.5, -0.1], 10)
Beispiel #17
0
def tourist(steps=25,shots=5,speed=.25):
    ZumoButton().wait_for_press()
    rs = ReflectanceSensors(); m = Motors(); c = Camera()
    for i in range(steps):
        random_step(m,speed=speed,duration=0.5)
        vals = rs.update()
        if sum(vals) < 1:  # very dark area
            im = shoot_panorama(c,m,shots)
            im.dump_image('vacation_pic'+str(i)+'.jpeg')
Beispiel #18
0
    def operationalize(self):
        #Oppretter motors-objekt
        m = Motors()
        #henter inn recommendation fra arbitrator og plugger inn i motors
        m.set_value(self.values, 0.5)

        #Går gjennom listen og bruker den til å utføre de forskjellige handlingene fra motors
        for i in range(len(self.motors)):
            self.motors[i].set_value(self.values, 0.5)
Beispiel #19
0
def bbrun():

    # Main function sets up motobs/sensobs and behavors
    sensobs = []
    # Calibration begins here. Students should slowly spin the robot around the
    # line in a circle trying hard not to lift it up off the ground. As of now,
    # calibration lasts 15 seconds which is plenty of time. Without proper
    # calibration, the line following behavior may not work properly.
    # Calibration determines the maximum and minimum values found by each sensor,
    # which is necessary for returning the normalized values (reals from 0 to 1)
    # of each sensor.
    # Edit: I have turned off calibration for now, because hard-coding the configuration
    # is working good enough and I don't have to use time on calibrating under testing
    motors = Motors()
    motors.stop()
    sleep(2)

    # To exit the program properly on keyboardinterrupt
    def signal_handler(signal, frame):
        motors.stop()
        GPIO.cleanup()
        sys.exit(0)
    signal.signal(signal.SIGINT, signal_handler)

    reflectanceSensob = bbc.Sensob(ReflectanceSensors())
    cameraSensob = bbc.Sensob(Camera())
    sensobs.append(reflectanceSensob)
    sensobs.append(cameraSensob)
    motob = bbc.Motob(motors)
    bbcon = RobotController(None) # None agent
    # Create the behaviors
    # The behaviors has priorities of 3,3,5,and 1 respectively
    followLineBehavior = FollowLine(bbcon, sensobs[0:1])
    camera_search_behavior = Camera_search(bbcon, sensobs[1:2])
    stopFollowingLineBehavior = StopFollowLine(bbcon, sensobs[0:1])
    wanderBehavior = Wander(bbcon)

    bbcon.add_behavior(followLineBehavior)
    bbcon.add_behavior(stopFollowingLineBehavior)
    bbcon.add_behavior(wanderBehavior)
    bbcon.add_behavior(camera_search_behavior)

    bbcon.add_motob(motob)
    bbcon.add_sensob(reflectanceSensob)
    bbcon.add_sensob(cameraSensob)

    i = 0
    while not bbcon.is_blob_found():
          print("Iteration " + str(i))
          i += 1
          print("Found gate: " + str(bbcon.found_gate))
          bbcon.run_one_timestep()


    # Called at the end
    motors.stop()
Beispiel #20
0
 def __init__(self):
     self.motor = Motors()  #list of all motors whose values it sets
     self.value = []  #most recent motor recommendation sent to the motob.
     self.motorsettings = {
         'f': self.motor.forward,
         'b': self.motor.backward,
         'l': self.motor.left,
         'r': self.motor.right,
         's': self.motor.stop
     }
Beispiel #21
0
 def __init__(self):
     self.motor = Motors()
     self.value = -1
     self.speed = 0.35
     self.funcs = {
         "left": self.left,
         "right": self.right,
         "random": self.random,
         "rewind": self.rewind
     }
Beispiel #22
0
def dancer():
    ZumoButton().wait_for_press()
    m = Motors()
    m.forward(0.2, 3)
    m.backward(0.2, 3)
    m.right(0.5, 3)
    m.left(0.5, 3)
    m.backward(0.3, 2.5)
    m.set_value([0.5, 0.1], 10)
    m.set_value([-0.5, -0.1], 10)
Beispiel #23
0
    def __init__(self, host, port):
        self.host = host
        self.port = port

        self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.socket.bind((host, port))
        self.socket.listen(1)
        self.client_socket = self.socket.accept()[0]
        Motors()
        self.listen()
Beispiel #24
0
    def __init__(self, a_star=None):
        self.a_star = a_star or AStar()
        self.encoders = Encoders(self.a_star)
        self.odometer = Odometer(self.encoders)
        self.motors = Motors(self.a_star, self.odometer)

        self.camera = Camera()
        self.log = logging.getLogger('romi')

        self.motors.stop()
Beispiel #25
0
def dancer():
    ZumoButton().wait_for_press()
    m = Motors()
    m.forward(.2, 3)
    m.forward(.3, 2)
    m.right(.5, 3)
    m.left(.5, 3)
    m.backward(.3, 2.5)
    m.set_value([.5, .1], 10)
    m.set_value([-.5, -.1], 10)
Beispiel #26
0
def main():
    gamepad = Gamepad()
    motors = Motors()

    iks = []

    for i in range(4):
        iks.append(DeltaIK())

    controller = ManualController(iks)

    angles = 8 * [0.0]
    kPs = 8 * [0.1]

    frametime = 1.0 / 30.0

    print("Ready.")

    while True:
        try:
            s = time.time()

            if not gamepad.process_events():
                print("Controller disconnected, waiting for reconnect...")

                while not gamepad.reconnect():
                    time.sleep(0.5)

                print("Controller reconnected.")

            if gamepad.is_a_down():
                print("Shutdown button (A) has been pressed, shutting down...")
                break

            pad_y = gamepad.get_left_thumbstick_y()

            controller.step(pad_y, frametime)

            # Map IK leg results to motors
            for i in range(4):
                angles[legMap[i]
                       [0]] = legDirs[i][0] * (iks[i].angle -
                                               (np.pi / 2.0 - iks[i].A))
                angles[legMap[i]
                       [1]] = legDirs[i][1] * (iks[i].angle +
                                               (np.pi / 2.0 - iks[i].A))

            motors.sendCommands(angles, kPs)

            time.sleep(max(0, frametime - (time.time() - s)))
        except Exception as e:
            print(e)
            break

    print("-- Program at End --")
Beispiel #27
0
async def update_motors():
    global cmd
    m = Motors()
    m.set(0, 0)
    while True:
        if time.time() - cmd['time'] > p.motor_command_timeout:
            m.set(0, 0)
        else:
            m.set(cmd['forward'], cmd['turn'])

        await asyncio.sleep(0.05)
Beispiel #28
0
def shoot_panorama(shots=5):
    camera = Camera()
    motors = Motors()
    s = 1
    im = IMR.Imager(image=camera.update()).scale(s, s)
    rotation_time = 3 / shots  # At a speed of 0.5(of max), it takes about 3 seconds to rotate 360 degrees
    for i in range(shots - 1):
        motors.right(0.5, rotation_time)
        im = im.concat_horiz(IMR.Imager(image=camera.update()))
    im.dump_image("/root/Oving6_Plab/basic_robot/bilder/test.png")
    return im
Beispiel #29
0
    def __init__(self, debug=False):
        self.motor = Motors()
        self.behaviors = []
        self.debug = debug

        self.sensobs = {
            Camera(img_width=IMG_WIDTH, img_height=IMG_HEIGHT): None,
            Ultrasonic(): None,
            ReflectanceSensors(): None
        }
        self.arbitrator = None
Beispiel #30
0
def explorer(dist=10):
    m = Motors()
    u = Ultrasonic()
    while u.update() > dist:
        m.forward(0.2, 0.2)
    m.backward(.1, 0.5)
    m.left(.5, 3)
    m.right(.5, 3.5)
    sleep(2)
    while u.update() < dist * 5:
        m.backward(.2, 0.2)
    m.left(.75, 5)