コード例 #1
0
ファイル: beh.py プロジェクト: kgmstwo/THBCP
def bump_beh(tv):
    # act on the information from the message.  Note that this might be 
    # information stored from the last message we received, because message 
    # information remains active for a while
    global bump_time
    global bump_state
    bump_bits = rone.bump_sensors_get_value()

    # check bump sensors
    if bump_left_get_value(bump_bits):
        bump_state = BUMP_STATE_TURN_RIGHT
        bump_time = sys.time() + BUMP_TIME_SIDE 
    elif bump_right_get_value(bump_bits):
        bump_state = BUMP_STATE_TURN_LEFT
        bump_time = sys.time() + BUMP_TIME_SIDE 
    elif bump_front_get_value(bump_bits):
        bump_state = BUMP_STATE_ROTATE
        bump_time = sys.time() + BUMP_TIME_ROTATE
    
    if sys.time() > bump_time:
        bump_state = BUMP_STATE_IDLE

    # control the robot
    beh_out = BEH_INACTIVE
    if bump_state == BUMP_STATE_TURN_RIGHT:
        beh_out = (0, -tv * TVRV_RATIO, True)
    elif bump_state == BUMP_STATE_TURN_LEFT:
        beh_out = (0, tv * TVRV_RATIO, True)
    elif bump_state == BUMP_STATE_ROTATE:
        beh_out = (0, tv * TVRV_RATIO, True)
        
    return beh_out
コード例 #2
0
def bump_beh(tv):
    # act on the information from the message.  Note that this might be
    # information stored from the last message we received, because message
    # information remains active for a while
    global bump_time
    global bump_state
    bump_bits = rone.bump_sensors_get_value()

    # check bump sensors
    if bump_left_get_value(bump_bits):
        bump_state = BUMP_STATE_TURN_RIGHT
        bump_time = sys.time() + BUMP_TIME_SIDE
    elif bump_right_get_value(bump_bits):
        bump_state = BUMP_STATE_TURN_LEFT
        bump_time = sys.time() + BUMP_TIME_SIDE
    elif bump_front_get_value(bump_bits):
        bump_state = BUMP_STATE_ROTATE
        bump_time = sys.time() + BUMP_TIME_ROTATE

    if sys.time() > bump_time:
        bump_state = BUMP_STATE_IDLE

    # control the robot
    beh_out = BEH_INACTIVE
    if bump_state == BUMP_STATE_TURN_RIGHT:
        beh_out = (0, -tv * TVRV_RATIO, True)
    elif bump_state == BUMP_STATE_TURN_LEFT:
        beh_out = (0, tv * TVRV_RATIO, True)
    elif bump_state == BUMP_STATE_ROTATE:
        beh_out = (0, tv * TVRV_RATIO, True)

    return beh_out
コード例 #3
0
ファイル: Wall_Follow.py プロジェクト: va17/THBCP
def wall_follow_demo():
    velocity.init(0.22, 40, 0.5, 0.1)
    leds.init()
    pose.init()
    motion.init()
    neighbors.init(NBR_PERIOD)

    state = STATE_IDLE
    wall_time = 0
    
    while True:
        # Do updates
        leds.update()
        pose.update()
        velocity.update()
        new_nbrs = neighbors.update()
        
        nbrList = neighbors.get_neighbors()
        tv = 0
        rv = 0

        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if new_nbrs:
                print "idle"
            if rone.button_get_value('r'):
                state = STATE_LOOK_FOR_WALL
            
        elif state == STATE_LOOK_FOR_WALL:
            leds.set_pattern('r', 'blink_fast', LED_BRIGHTNESS)
            if new_nbrs:
                print "look for wall"
            tv = MOTION_TV
            obs = neighbors.get_obstacles() 
            if (obs != None):
                state = STATE_WALL_FOLLOW                
            
        elif state == STATE_WALL_FOLLOW:
            leds.set_pattern('b', 'blink_fast', LED_BRIGHTNESS)
            if new_nbrs:
                print "wall follow"
            # follow the wall
            (tv, rv, active) = wall_follow(MOTION_TV / 2)
            if active == True:
                wall_time = sys.time()
            if sys.time() > (wall_time + WALL_TIMEOUT):
                state = STATE_LOOK_FOR_WALL
                
        # end of the FSM
                        
        # set the velocities
        velocity.set_tvrv(tv, rv)
        
        #set the message
        hba.set_msg(0, 0, 0)
コード例 #4
0
def init(nbr_period):
    _nbr_state['time_ir_xmit'] = sys.time()
    _nbr_state['time_ir_xmit_offset'] = 0
    _nbr_state['nbr_period'] = nbr_period
    _nbr_state['nbr_timeout'] = 3 * nbr_period
    _nbr_state['message'] = ''
    _nbr_state['xmit_enable'] = True
    _nbr_state['nbr_list'] = []
    _nbr_state['obstacles'] = None
    _nbr_state['obstacles_time'] = sys.time()
コード例 #5
0
ファイル: neighborsX.py プロジェクト: rdspring1/comp551
def init(nbr_period, compute_bearing_func, compute_ornt_func):
    _nbr_state['time_ir_xmit'] = sys.time()
    _nbr_state['time_ir_xmit_offset'] = 0
    _nbr_state['nbr_period'] = nbr_period
    _nbr_state['nbr_timeout'] = 3 * nbr_period
    _nbr_state['message'] = ''
    _nbr_state['xmit_enable'] = True
    _nbr_state['nbr_list'] = []
    _nbr_state['obstacles'] = None
    _nbr_state['obstacles_time'] = sys.time()
    global _compute_brg
    global _compute_ornt
    _compute_brg = compute_bearing_func
    _compute_ornt = compute_ornt_func
コード例 #6
0
ファイル: HXExport.py プロジェクト: paulfitz/blender2haxe
def main():
    # Saves the editmode state and go's out of
    # editmode if its enabled, we cant make
    # changes to the mesh data while in editmode.
    is_editmode = Window.EditMode()
    if is_editmode:
        Window.EditMode(0)

    Window.WaitCursor(1)
    t = sys.time()

    # Restore editmode if it was enabled
    if is_editmode:
        Window.EditMode(1)

    print "ActionScript 3.0 Exporter Script finished in %.2f seconds" % (sys.time() - t)
    Window.WaitCursor(0)
コード例 #7
0
def FTL_leader():
    global radio_msg_time, FTL_leader_tv, FTL_leader_rv
    radio_msg = radio_get_message()

    if radio_msg != None:
        # we have a message! put lights in active mode
        leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
        radio_msg_time = sys.time()
        (FTL_leader_tv, FTL_leader_rv) = leader_motion_controller(radio_msg)
    else:
        # no message. check for radio message timeout
        if sys.time() > (radio_msg_time + REMOTE_XMIT_DELAY * 3):
            # message timeout.  stop the motors
            FTL_leader_tv = 0
            FTL_leader_rv = 0
        leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
    velocity.set_tvrv(FTL_leader_tv, FTL_leader_rv)
コード例 #8
0
ファイル: hba.py プロジェクト: kgmstwo/THBCP
def leds_blink_all(brightness):
    if (sys.time() % 500) < 250:
        rone.led_set_group('r', brightness)
        rone.led_set_group('g', brightness)
        rone.led_set_group('b', brightness)
    else:
        rone.led_set_group('r', 0)
        rone.led_set_group('g', 0)
        rone.led_set_group('b', 0)
コード例 #9
0
ファイル: Flock_Demo.py プロジェクト: va17/THBCP
def receive_beh():
    global receive_msg_time, receive_msg_tv, receive_msg_rv
    radio_msg = rone.radio_get_message()
    active = True
    if radio_msg != None:
        # we have a message! put lights in active mode
        leds.set_pattern('g','blink_fast',LED_BRIGHTNESS)
        receive_msg_time = sys.time()
        (receive_msg_tv, receive_msg_rv) = remote_motion_controller(radio_msg)
    else:
        # no message. check for radio message timeout
        if sys.time() > (receive_msg_time + REMOTE_XMIT_DELAY * 3):
            # message timeout.  stop the motors
            receive_msg_tv = 0
            receive_msg_rv = 0
            active = False
        leds.set_pattern('g','circle',LED_BRIGHTNESS)
    return (receive_msg_tv, receive_msg_rv, active)
コード例 #10
0
ファイル: hba.py プロジェクト: va17/THBCP
def leds_blink_all(brightness):
    if (sys.time() % 500) < 250:
        rone.led_set_group('r', brightness)
        rone.led_set_group('g', brightness)
        rone.led_set_group('b', brightness)
    else:
        rone.led_set_group('r', 0)
        rone.led_set_group('g', 0)
        rone.led_set_group('b', 0)
コード例 #11
0
ファイル: pose.py プロジェクト: va17/THBCP
def init():
    global pose_state
    pose_state['ticksL'] = rone.encoder_get_ticks('l')
    pose_state['ticksR'] = rone.encoder_get_ticks('r')
    #pose_state['pose'] = (0.0, 0.0, 0.0)
    pose_state['x'] = 0.0
    pose_state['y'] = 0.0
    pose_state['theta'] = 0.0
    pose_state['odometer'] = 0.0
    pose_state['update_time'] = sys.time()
コード例 #12
0
ファイル: velocity.py プロジェクト: rdspring1/rone-testbed
def init(kff, kff_offset, kp, ki):
    _vcstate['l'] = {}
    _vcstate['r'] = {}

    # K terms
    _vcstate['kff'] = kff
    _vcstate['kff_offset'] = kff_offset
    _vcstate['kp'] = kp
    _vcstate['ki'] = ki
    _vcstate['update_time'] = sys.time()
    _vcstate['tv_ramp'] = 0
    _vcstate['tv_update_time'] = sys.time()

    for motor in ['l', 'r']:
        _vcstate[motor]['ticks'] = rone.encoder_get_ticks(motor) # Position
        _vcstate[motor]['time'] = sys.time() # Time
        _vcstate[motor]['iterm'] = 0.0 # Iterm
        _vcstate[motor]['goalvel'] = 0 # Goal velocity
        _vcstate[motor]['vel'] = 0 # Current velocity
コード例 #13
0
ファイル: velocity.py プロジェクト: kgmstwo/THBCP
def init(kff, kff_offset, kp, ki):
    _vcstate["l"] = {}
    _vcstate["r"] = {}

    # K terms
    _vcstate["kff"] = kff
    _vcstate["kff_offset"] = kff_offset
    _vcstate["kp"] = kp
    _vcstate["ki"] = ki
    _vcstate["update_time"] = sys.time()
    _vcstate["tv_ramp"] = 0
    _vcstate["tv_update_time"] = sys.time()

    for motor in ["l", "r"]:
        _vcstate[motor]["ticks"] = rone.encoder_get_ticks(motor)  # Position
        _vcstate[motor]["time"] = sys.time()  # Time
        _vcstate[motor]["iterm"] = 0.0  # Iterm
        _vcstate[motor]["goalvel"] = 0  # Goal velocity
        _vcstate[motor]["vel"] = 0  # Current velocity
コード例 #14
0
ファイル: velocity.py プロジェクト: rdspring1/comp551
def init(kff, kff_offset, kp, ki):
    _vcstate['l'] = {}
    _vcstate['r'] = {}

    # K terms
    _vcstate['kff'] = kff
    _vcstate['kff_offset'] = kff_offset
    _vcstate['kp'] = kp
    _vcstate['ki'] = ki
    _vcstate['update_time'] = sys.time()
    _vcstate['tv_ramp'] = 0
    _vcstate['tv_update_time'] = sys.time()

    for motor in ['l', 'r']:
        _vcstate[motor]['ticks'] = rone.encoder_get_ticks(motor)  # Position
        _vcstate[motor]['time'] = sys.time()  # Time
        _vcstate[motor]['iterm'] = 0.0  # Iterm
        _vcstate[motor]['goalvel'] = 0  # Goal velocity
        _vcstate[motor]['vel'] = 0  # Current velocity
コード例 #15
0
ファイル: poseX.py プロジェクト: rdspring1/comp551
def update():
    global pose_state
    global _update_pose
    current_time = sys.time()
    update_time = pose_state['update_time']
    if current_time > update_time:
        update_time += POSE_UPDATE_PERIOD
        # advance time if there have been delays in calling update
        if update_time < current_time:
            update_time = current_time + POSE_UPDATE_PERIOD
        pose_state['update_time'] = update_time
        
        _update_pose(pose_state)
コード例 #16
0
def node_initialize(node):
    sp = pxssh.pxssh()
    counter = 0
    print(node["node-name"] + " -> Logging into SP")
    if (not sp.login(
            node["sp-ip"], node["user"], node["password"],
            auto_prompt_reset=False)):
        print(node["node-name"] + " -> SSH session failed on login.")
        print(str(sp))
    else:
        print(node["node-name"] + " -> SSH session login successful")

        # Switching to system console
        exec_ssh(sp, "^SP.*>", "system console", node["node-name"])
        exec_ssh(sp, None, "", node["node-name"])

        # Wait for loader prompt and boot into boot menu
        counter = 0
        while not exec_ssh(sp, "LOADER-[AB]{1}>", "boot_ontap menu", node["node-name"]) and \
          (counter < 12):
            print(node["node-name"] + " -> waiting 10 seconds...")
            counter = counter + 1
            sys.time(10)
        if (counter >= 12):
            print(node["node-name"] + " -> Could not set prompt. Exiting NOW!")
            sys.exit(1)

        # selecting option for initialize in boot menu
        counter = 0
        while not exec_ssh(sp, "Selection .*\?", "4", node["node-name"]) and \
         (counter < 12):
            print(node["node-name"] + " -> waiting 10 seconds...")
            counter = counter + 1
            sys.time(10)
        if (counter >= 12):
            print(node["node-name"] + " -> Could not set prompt. Exiting NOW!")
            sys.exit(1)

        # Confirming warnings
        counter = 0
        while not exec_ssh(sp, "Zero disks, reset config and install a new file system\?:", "y", node["node-name"]) and \
         (counter < 12):
            print(node["node-name"] + " -> waiting 10 seconds...")
            counter = counter + 1
            sys.time(10)
        if (counter >= 12):
            print(node["node-name"] + " -> Could not set prompt. Exiting NOW!")
            sys.exit(1)
        else:
            exec_ssh(
                sp,
                "This will erase all the data on the disks, are you sure\?:",
                "y", node["node-name"])

        exec_ssh(sp, None, "d", node["node-name"])
    print(node["node-name"] + " -> logging out...")
    sp.logout()
コード例 #17
0
ファイル: velocity.py プロジェクト: kgmstwo/THBCP
def set_tvrv(tv, rv):
    tv = int(tv)
    if sys.time() > _vcstate["tv_update_time"]:
        _vcstate["tv_update_time"] += _TV_UPDATE_PERIOD
        if tv < _vcstate["tv_ramp"]:
            _vcstate["tv_ramp"] -= TV_RAMP_DOWN
            if tv > _vcstate["tv_ramp"]:
                _vcstate["tv_ramp"] = tv
        if tv > _vcstate["tv_ramp"]:
            _vcstate["tv_ramp"] += TV_RAMP_UP
            if _vcstate["tv_ramp"] > tv:
                _vcstate["tv_ramp"] = tv
    tv = _vcstate["tv_ramp"]
    left_vel = tv - rv * WHEEL_BASE / 2000
    right_vel = tv + rv * WHEEL_BASE / 2000
    left_vel = clamp(left_vel, SPEED_MAX)
    right_vel = clamp(right_vel, SPEED_MAX)
    set("l", left_vel)
    set("r", right_vel)
コード例 #18
0
ファイル: velocity.py プロジェクト: rdspring1/comp551
def set_tvrv(tv, rv):
    tv = int(tv)
    if sys.time() > _vcstate['tv_update_time']:
        _vcstate['tv_update_time'] += _TV_UPDATE_PERIOD
        if tv < _vcstate['tv_ramp']:
            _vcstate['tv_ramp'] -= TV_RAMP_DOWN
            if tv > _vcstate['tv_ramp']:
                _vcstate['tv_ramp'] = tv
        if tv > _vcstate['tv_ramp']:
            _vcstate['tv_ramp'] += TV_RAMP_UP
            if _vcstate['tv_ramp'] > tv:
                _vcstate['tv_ramp'] = tv
    tv = _vcstate['tv_ramp']
    left_vel = tv - rv * WHEEL_BASE / 2000
    right_vel = tv + rv * WHEEL_BASE / 2000
    left_vel = clamp(left_vel, SPEED_MAX)
    right_vel = clamp(right_vel, SPEED_MAX)
    set('l', left_vel)
    set('r', right_vel)
コード例 #19
0
ファイル: velocity.py プロジェクト: rdspring1/comp551
def _velocity(motor):
    ## vel_goal in mm/s
    ## motor either 'l' or 'r'
    ## iterm_old passed in from previous function runs (sum of the errors * ki)
    ## ticks_old is the old position of the given wheel
    ## time_old is the time of the last reading of the encoders

    vel_goal = _vcstate[motor]['goalvel']
    iterm_old = _vcstate[motor]['iterm']
    ticks_old = _vcstate[motor]['ticks']
    time_old = _vcstate[motor]['time']

    # compute distance
    ticks_new = rone.encoder_get_ticks(motor)
    distance = _compute_distance(ticks_new, ticks_old)

    # compute velocity_controller
    time_new = sys.time()
    time_delta = time_new - time_old
    velocity = _compute_velocity(distance, time_delta)
    _vcstate[motor]['vel'] = velocity
    # some debug printing.  Don't leave it in, it slows down the computer

    # compute feedback terms
    error = vel_goal - velocity
    feedforward_term = _feedforward_compute(vel_goal)
    proportional_term = _proportional_compute(error)
    integral_term = _integral_compute(error, iterm_old)
    pwm = feedforward_term + proportional_term + integral_term
    pwm = int(pwm)
    pwm = clamp(pwm, 100)
    rone.motor_set_pwm(motor, pwm)

    #Some example debugging output.  Don't print this always, it will slow things down too much
    #print 'motor=%s ff_term=%5.1f  i_term=%5.1f  pwm=%3d' % (motor, float(feedforward_term), float(integral_term), pwm)

    # update old values
    _vcstate[motor]['ticks'] = ticks_new
    _vcstate[motor]['time'] = time_old = time_new
    _vcstate[motor]['iterm'] = integral_term
コード例 #20
0
ファイル: velocity.py プロジェクト: kgmstwo/THBCP
def _velocity(motor):
    ## vel_goal in mm/s
    ## motor either 'l' or 'r'
    ## iterm_old passed in from previous function runs (sum of the errors * ki)
    ## ticks_old is the old position of the given wheel
    ## time_old is the time of the last reading of the encoders

    vel_goal = _vcstate[motor]["goalvel"]
    iterm_old = _vcstate[motor]["iterm"]
    ticks_old = _vcstate[motor]["ticks"]
    time_old = _vcstate[motor]["time"]

    # compute distance
    ticks_new = rone.encoder_get_ticks(motor)
    distance = _compute_distance(ticks_new, ticks_old)

    # compute velocity_controller
    time_new = sys.time()
    time_delta = time_new - time_old
    velocity = _compute_velocity(distance, time_delta)
    _vcstate[motor]["vel"] = velocity
    # some debug printing.  Don't leave it in, it slows down the computer

    # compute feedback terms
    error = vel_goal - velocity
    feedforward_term = _feedforward_compute(vel_goal)
    integral_term = _integral_compute(error, iterm_old)
    proportional_term = _proportional_compute(error)
    pwm = feedforward_term + proportional_term + integral_term
    pwm = int(pwm)
    pwm = clamp(pwm, 100)
    rone.motor_set_pwm(motor, pwm)

    # Some example debugging output.  Don't print this always, it will slow things down too much
    # print 'motor=%s ff_term=%5.1f  i_term=%5.1f  pwm=%3d' % (motor, float(feedforward_term), float(integral_term), pwm)

    # update old values
    _vcstate[motor]["ticks"] = ticks_new
    _vcstate[motor]["time"] = time_old = time_new
    _vcstate[motor]["iterm"] = integral_term
コード例 #21
0
ファイル: pose.py プロジェクト: va17/THBCP
def update():
    global pose_state
    current_time = sys.time()
    update_time = pose_state['update_time']
    if current_time > update_time:
        update_time += POSE_UPDATE_PERIOD
        # advance time if there have been delays in calling update
        if update_time < current_time:
            update_time = current_time + POSE_UPDATE_PERIOD
        pose_state['update_time'] = update_time
        
        # Compute the linear distance traveled by each wheel in millimeters
        ticksL = rone.encoder_get_ticks('l')
        ticksR = rone.encoder_get_ticks('r')
        distL = float(velocity.encoder_delta_ticks(ticksL, pose_state['ticksL'])) * 0.0625
        distR = float(velocity.encoder_delta_ticks(ticksR, pose_state['ticksR'])) * 0.0625
        pose_state['ticksL'] = ticksL
        pose_state['ticksR'] = ticksR 
    
        # Compute the distance traveled by the center of the robot in millimeters
        dist = (distR + distL) / 2.0
        pose_state['odometer'] += abs(dist)
        
        # compute the arc angle in radians
        # use the small angle approximation: arctan(theta) ~ theta
        delta_theta = (distR - distL) / (WHEEL_BASE);
#        (x, y, theta) = pose_state['pose']
#        x = x + dist * math.cos(theta)
#        y = y + dist * math.sin(theta)
#        theta = math2.normalize_theta(theta + delta_theta)
#        pose_state['pose'] = (x, y, theta)

        theta = pose_state['theta']
        pose_state['x'] += dist * math.cos(theta)
        pose_state['y'] += dist * math.sin(theta)
        pose_state['theta'] = math2.normalize_angle(theta + delta_theta)
コード例 #22
0
import sys;
import list;
import dict;
import string;
import ipm;

def nulladd(x):
	return(x+1);

def nulllib(y):
	print ;
	return(y - 355);

j=0;
### dir(__builtins__);
print sys.time(); # will return zero... not implemented yet X-D

a="hello world!";
print "=== check: string";
"hello world!";
a;

print "=== check: print string";
print "hello world!";
print a;
print len("hello world!");
print len(a);
##print length(a[2:7]);  ## slicees are not implemented yet 
##print a[2:7];  ## slicees are not implemented yet 

print "=== check: print number";
コード例 #23
0
ファイル: velocity.py プロジェクト: kgmstwo/THBCP
def update():
    if sys.time() > _vcstate["update_time"]:
        _vcstate["update_time"] += _VEL_UPDATE_PERIOD
        _velocity("l")
        _velocity("r")
コード例 #24
0
ファイル: test.py プロジェクト: 01iv3r/OpenPilot
#mb = mb[0]
import flightplanstatus
#ma = sys.heap()
#ma = ma[0]
#print('import flightplanstatus')
#print(mb-ma)
#mb = sys.heap()
#mb = mb[0]
import mixersettings
#ma = sys.heap()
#ma = ma[0]
#print('import mixersettings')
#print(mb-ma)

n = 0
timenow = sys.time()
fpStatus = flightplanstatus.FlightPlanStatus()

while n < 120:
	n = n+1 
	#openpilot.debug(n, timenow)
	fpStatus.read()
	fpStatus.Debug.value[0] = n
	fpStatus.Debug.value[1] = timenow
	fpStatus.write()
	timenow = openpilot.delayUntil(timenow, 1000)
	if openpilot.hasStopRequest():
		sys.exit()


コード例 #25
0
ファイル: fall.py プロジェクト: va17/THBCP
def fall():
    found_flower = False
    start_time = 0
    target_theta = 0
    my_color = -1
    beh.init(0.22, 40, 0.5, 0.1)
    state = STATE_IDLE
    color = 'r'  #for flowers only

    def wander():
        state = STATE_WANDER

    def collect_pollen():
        state = STATE_COLLECT_POLLEN
        start_time = sys.time()

    def align_with(target):
        target_theta = target
        pose.set_pose(0, 0, 0)
        state = STATE_ALIGN
        start_time = sys.time()

    #motion_start_odo = pose.get_odometer()

    while True:
        beh.init(0.22, 40, 0.5, 0.1)

        new_nbrs = beh.update()
        nbrList = neighbors.get_neighbors()
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE

        #FINITE STATE MACHINE
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MOVE_TO_FLOWER
            if rone.button_get_value('b'):
                state = STATE_QUEEN
            if new_nbrs:
                print "idle"

        elif state == STATE_WANDER:  #run forward, avoid direction of neighbors
            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            if nav_tower == None:
                state = STATE_RETURN_TO_BASE
            else:
                beh_out = beh.avoid_nbr(nav_tower, MOTION_TV)

                (flower, color) = detflower(nbrList)
                if flower != None:
                    state = STATE_MOVE_TO_FLOWER

        elif state == STATE_MOVE_TO_FLOWER:
            leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
            (flower, color) = detflower(nbrList)
            if flower != None:
                if (neighbors.get_nbr_range_bits(flower) >
                        6) or (beh.bump_angle_get() != None):
                    collect_pollen()  #collect pollen if we bump or get close
                else:
                    #otherwise keep following that flower
                    beh_out = beh.follow_nbr(flower, MOTION_TV)

        elif state == STATE_COLLECT_POLLEN:
            motion_start_odo = pose.get_odometer()
            if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
                state = STATE_RETURN_TO_BASE
                found_flower = True
            elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):
                tv = -MOTION_TV
                rv = 0
                beh_out = beh.tvrv(tv, rv)
                turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)

            elif sys.time() < (turn_start_time + TURN_TIME):
                tv = 40
                rv = -MOTION_RV
                beh_out = beh.tvrv(tv, rv)

            else:
                tv = MOTION_TV
                rv = (MOTION_RV - 300)
                beh_out = beh.tvrv(tv, rv)

        elif state == STATE_RETURN_TO_BASE:
            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = find_queen(nbrList)
            if (nav_tower == None) and (queen == None):
                beh_out = (-MOTION_TV, 0, True)
            elif (nav_tower != None) and (queen == None):
                beh_out = beh.follow_nbr(nav_tower)
            elif neighbors.get_nbr_range_bits(queen) > 2:
                beh_out = beh.follow_nbr(queen, MOTION_TV)
            elif found_flower:
                state = STATE_RECRUIT
                start_time = sys.time()
            else:
                state = STATE_FOLLOW
                start_time = sys.time()

        elif state == STATE_FOLLOW:
            recruiter = find_recruiter()
            if recruiter == None:
                beh_out = beh.BEH_INACTIVE
                if sys.time() > (follow_start_time + FOLLOW_TIME):
                    wander()
            else:
                bearing = neighbors.get_nbr_bearing(recruiter)
                orientation = neighbors.get_nbr_orientation(recruiter)
                align_with(math.pi + bearing - orientation)

        elif state == STATE_GO:
            flower = detflower()
            if not flower == None:
                state = STATE_MOVE_TO_FLOWER
            beh_out = beh.tvrv(MOTION_TV, 0)

        elif state == STATE_RECRUIT:
            if sys.time() > (recruit_start_time + RECRUIT_TIME):
                align_with(pose.get_theta() - math.pi)

        elif state == STATE_ALIGN:
            tv = 0
            heading_error = math.normalize_angle(pose.get_theta() -
                                                 target_theta)
            rv = ROTATE_RV_GAIN * heading_error
            beh_out = beh.tvrv(tv, rv)
            # you could actually do a running average in the list here
            small_error = hba.average_error_check(heading_error, [],
                                                  HEADING_ERROR_LIMIT,
                                                  new_nbrs)
            if new_nbrs:
                print "error", error_list
            if small_error:
                state = STATE_GO
        #END OF FINITE STATE MACHINE

        bump_beh_out = beh.bump_beh(MOTION_TV)
        if state not in [
                STATE_RETURN_TO_BASE, STATE_COLLECT_POLLEN, STATE_RECRUIT
        ]:
            beh_out = beh.subsume([beh_out, bump_beh_out])
        beh.motion_set(beh_out)
        hba.set_msg(state, my_color, 0)
コード例 #26
0
ファイル: fall.py プロジェクト: va17/THBCP
 def collect_pollen():
     state = STATE_COLLECT_POLLEN
     start_time = sys.time()
コード例 #27
0
ファイル: t090.py プロジェクト: AGM1968/python-on-a-chip
# it under the terms of the GNU LESSER GENERAL PUBLIC LICENSE Version 2.1.
#
# Python-on-a-Chip is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# A copy of the GNU LESSER GENERAL PUBLIC LICENSE Version 2.1
# is seen in the file COPYING up one directory from this.

#
# Test for Issue #90: Create new lib function to return system time
# Ensure that time passes
#

import sys

t0 = sys.time()

print "t0 = ", t0

print "killing time..."
i=0
while i < 10000:
    j=0
    while j < 100:
        j += 1
    if i%100 == 0:
        print i / 100, " ",
    i += 1
print

t1 = sys.time()
コード例 #28
0
ファイル: fall.py プロジェクト: kgmstwo/THBCP
 def collect_pollen():
     state = STATE_COLLECT_POLLEN
     start_time = sys.time()
コード例 #29
0
ファイル: t090.py プロジェクト: ArtemovSA/PythonOnChip
# it under the terms of the GNU LESSER GENERAL PUBLIC LICENSE Version 2.1.
#
# Python-on-a-Chip is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# A copy of the GNU LESSER GENERAL PUBLIC LICENSE Version 2.1
# is seen in the file COPYING up one directory from this.

#
# Test for Issue #90: Create new lib function to return system time
# Ensure that time passes
#

import sys

t0 = sys.time()

print "t0 = ", t0

print "killing time..."
i = 0
while i < 10000:
    j = 0
    while j < 100:
        j += 1
    if i % 100 == 0:
        print i / 100, " ",
    i += 1
print

t1 = sys.time()
コード例 #30
0
ファイル: fall.py プロジェクト: va17/THBCP
 def align_with(target):
     target_theta = target
     pose.set_pose(0, 0, 0)
     state = STATE_ALIGN
     start_time = sys.time()
コード例 #31
0
ファイル: Flock_Demo.py プロジェクト: va17/THBCP
    #print 'radio msg:', radio_msg  # debugging code - comment out when this is working

    if 'g' in radio_msg:
        # move forward
        tv = FTL_TV
    if 'r' in radio_msg:
        # rotate left when red button pressed
        rv = FTL_RV
    elif 'b' in radio_msg:
        # rotate right when blue button pressed
        rv = -FTL_RV
    return (tv, rv)



receive_msg_time = sys.time()
receive_msg_tv = 0
receive_msg_rv = 0

def receive_beh():
    global receive_msg_time, receive_msg_tv, receive_msg_rv
    radio_msg = rone.radio_get_message()
    active = True
    if radio_msg != None:
        # we have a message! put lights in active mode
        leds.set_pattern('g','blink_fast',LED_BRIGHTNESS)
        receive_msg_time = sys.time()
        (receive_msg_tv, receive_msg_rv) = remote_motion_controller(radio_msg)
    else:
        # no message. check for radio message timeout
        if sys.time() > (receive_msg_time + REMOTE_XMIT_DELAY * 3):
コード例 #32
0
ファイル: robot.py プロジェクト: ArtemovSA/ILC_project
    else:
        left_wheel.setCounts(left_wheel_forward)
        right_wheel.setCounts(right_wheel_forward)

    # Print out line following diags
    if line_sensor_left.get():
        print "X",
    else:
        print ".",
    if line_sensor_middle.get():
        print "X",
    else:
        print ".",
    if line_sensor_right.get():
        print "X"
    else:
        print "."

    # Delay some
    time = sys.time()
    while time + 10 > sys.time():
        pass
    







コード例 #33
0
ファイル: Flower_Motion.py プロジェクト: kgmstwo/THBCP
def flower_motion():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()
        
        nbrList = neighbors.get_neighbors()
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE
            
        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MOVE_TO_FLOWER
            if new_nbrs:
                print "idle"
            
        elif state == STATE_MOVE_TO_FLOWER:
            leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
            if new_nbrs:
                print "move to flower"
           
            # Move towards the flower until you bump into it
            # for this demo, assume the first robot on the list is a flower
##            flower = nbrList_getFirstRobot(nbrList)
            (color,nbr) = detflower(nbrList)
            flower = nbr
            if flower != None:
                # Stop if we get close or bump into the flower
                #if neighbors.get_nbr_close_range(flower):
                if (neighbors.get_nbr_range_bits(flower) > 6) or (beh.bump_angle_get() != None):
                    state = STATE_COLLECT_POLLEN
                    collect_pollen_start_time = sys.time()
                else:
                    # Move to the flower
                    beh_out = beh.follow_nbr(flower, MOTION_TV)
                    #print beh_out

        elif state == STATE_COLLECT_POLLEN:
            # this is where you will put your clever pollen collection code
            # we will just wait for a second, then leave. (this will not collect very much pollen)
            leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
            if new_nbrs:
                print "collect"
            
            # Timeout after 5 seconds
            if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
                state = STATE_MOVE_AWAY_FLOWER
            
            elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):    
                tv = -MOTION_TV
                rv = 0
                beh_out = beh.tvrv(tv,rv) 
                turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)
                
            elif sys.time() < (turn_start_time + TURN_TIME): 
                tv = 0
                rv = -MOTION_RV
            
            else: 
                tv = MOTION_TV
                rv = MOTION_RV
                beh_out = beh.tvrv(tv,rv)
            
        elif state == STATE_MOVE_AWAY_FLOWER:
            if new_nbrs:
                print "avoid flower"
            leds.set_pattern('r', 'blink_slow', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MOVE_TO_FLOWER

            # Move away from the flower until it is out of range
            flower = nbrList_getFirstRobot(nbrList)
            if flower != None:
                # Point away the flower
                beh_out = beh.avoid_nbr(flower, MOTION_TV)
            else:
                state = STATE_IDLE
                
        # end of the FSM
        bump_beh_out = beh.bump_beh(MOTION_TV)

        if state != STATE_COLLECT_POLLEN:
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        hba.set_msg(0, 0, 0)
コード例 #34
0
def update():
    global old_pattern
    current_time = sys.time()
    update_time = _leds_state['update_time']
    if current_time > update_time:
        update_time += LED_UPDATE_PERIOD
        # advance time if there have been delays in calling update
        if update_time < current_time:
            update_time = current_time + LED_UPDATE_PERIOD
        _leds_state['update_time'] = update_time
        
        counter = _leds_state['counter'] + 1
        _leds_state['counter'] = counter
        color = _leds_state['color']
        pattern = _leds_state['pattern']
        brightness = _leds_state['brightness']
        
        if _leds_state['pattern'] != 'manual':
            for c in 'rgb':
                if c not in color:
                    _led_set_group(c, 0)
    
        if pattern == 'group':
            _led_set_group(color, brightness)
        elif pattern == 'ramp_slow':
            idx = counter % BLINK_SLOW
            if idx < (BLINK_SLOW / 2):
                # ramp up
                b = brightness * idx / (BLINK_SLOW / 2)
            else:
                b = brightness * (BLINK_SLOW - idx) / (BLINK_SLOW / 2)
            _led_set_group(color, b)
        elif pattern == 'blink_slow':
            idx = counter % BLINK_SLOW
            if idx < (BLINK_SLOW / 2):
                _led_set_group(color, brightness)
            else:
                _led_set_group(color, 0)
        elif pattern == 'blink_fast':
            idx = counter % BLINK_FAST
            if idx < (BLINK_FAST / 2):
                _led_set_group(color, brightness)
            else:
                _led_set_group(color, 0)
        elif pattern == 'circle':
            idx = counter % 5
            led_map = rone._led_map[color]
            for i in range(5):
                if i == idx:
                    rone._led_set(led_map[i], brightness)
                else:
                    rone._led_set(led_map[i], 0)
        elif pattern == 'count':
            # display ints as counting on the lights
            idx = 0
            for c in 'rgb':
                if color[idx] == 0:
                    _led_set_group(c, 0)
                else:
                    led_map = rone._led_map[c]
                    for i in range(5):
                        if i < color[idx]:
                            rone._led_set(led_map[i], brightness)
                        else:
                            rone._led_set(led_map[i], 0)
                idx += 1
        elif pattern == 'manual':
            pass
        
        old_pattern = pattern
コード例 #35
0
ファイル: test.py プロジェクト: DovahDoVolom/Test
def main():
  sys.time(h,m,d)
コード例 #36
0
ファイル: fall.py プロジェクト: kgmstwo/THBCP
 def align_with(target):
     target_theta = target
     pose.set_pose(0,0,0)
     state = STATE_ALIGN
     start_time = sys.time()
コード例 #37
0
ファイル: beh.py プロジェクト: kgmstwo/THBCP
    return ((bump_bits & 7) > 0)

def bump_front_get_value(bump_bits):
    return (bump_bits == 129)

def bump_right_get_value(bump_bits):
    return ((bump_bits & 224) > 0)

BUMP_TIME_SIDE = 300
BUMP_TIME_ROTATE = 500
BUMP_STATE_IDLE = 0
BUMP_STATE_TURN_RIGHT = 1
BUMP_STATE_TURN_LEFT = 2
BUMP_STATE_ROTATE = 3

bump_time = sys.time()
bump_state = BUMP_STATE_IDLE

def bump_beh(tv):
    # act on the information from the message.  Note that this might be 
    # information stored from the last message we received, because message 
    # information remains active for a while
    global bump_time
    global bump_state
    bump_bits = rone.bump_sensors_get_value()

    # check bump sensors
    if bump_left_get_value(bump_bits):
        bump_state = BUMP_STATE_TURN_RIGHT
        bump_time = sys.time() + BUMP_TIME_SIDE 
    elif bump_right_get_value(bump_bits):
コード例 #38
0
ファイル: hba.py プロジェクト: kgmstwo/THBCP
def winter_time_keeper(initial_time):
    score_time = sys.time()-initial_time
    return score_time
コード例 #39
0
ファイル: velocity.py プロジェクト: rdspring1/comp551
def update():
    if sys.time() > _vcstate['update_time']:
        _vcstate['update_time'] += _VEL_UPDATE_PERIOD
        _velocity('l')
        _velocity('r')
コード例 #40
0
ファイル: fall.py プロジェクト: kgmstwo/THBCP
def fall(): 
    found_flower = False
    start_time = 0
    target_theta = 0
    my_color = -1
    beh.init(0.22, 40, 0.5, 0.1)
    state = STATE_IDLE
    color = 'r' #for flowers only

    def wander():
        state = STATE_WANDER
    def collect_pollen():
        state = STATE_COLLECT_POLLEN
        start_time = sys.time()
    def align_with(target):
        target_theta = target
        pose.set_pose(0,0,0)
        state = STATE_ALIGN
        start_time = sys.time()

    #motion_start_odo = pose.get_odometer()

    while True:
        beh.init(0.22, 40, 0.5, 0.1)

        new_nbrs = beh.update()
        nbrList = neighbors.get_neighbors()       
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE

        #FINITE STATE MACHINE
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MOVE_TO_FLOWER
            if rone.button_get_value('b'):
                state = STATE_QUEEN
            if new_nbrs:
                print "idle"

        elif state == STATE_WANDER: #run forward, avoid direction of neighbors
            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            if nav_tower == None:
                state = STATE_RETURN_TO_BASE
            else:
                beh_out = beh.avoid_nbr(nav_tower, MOTION_TV)

                (flower, color) = detflower(nbrList)
                if flower != None:
                    state = STATE_MOVE_TO_FLOWER

        elif state == STATE_MOVE_TO_FLOWER:
            leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
            (flower, color) = detflower(nbrList)
            if flower != None:
                if (neighbors.get_nbr_range_bits(flower) > 6) or (beh.bump_angle_get() != None):
                    collect_pollen() #collect pollen if we bump or get close
                else:
                #otherwise keep following that flower
                    beh_out = beh.follow_nbr(flower, MOTION_TV)

        elif state == STATE_COLLECT_POLLEN:
            motion_start_odo = pose.get_odometer()
            if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
                state = STATE_RETURN_TO_BASE
                found_flower = True
            elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):    
                tv = -MOTION_TV
                rv = 0
                beh_out = beh.tvrv(tv,rv) 
                turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)

            elif sys.time() < (turn_start_time + TURN_TIME): 
                tv = 40
                rv = -MOTION_RV
                beh_out = beh.tvrv(tv,rv)

            else: 
                tv = MOTION_TV
                rv = (MOTION_RV - 300)
                beh_out = beh.tvrv(tv,rv)

        elif state == STATE_RETURN_TO_BASE:
            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = find_queen(nbrList)
            if (nav_tower == None) and (queen == None):
                beh_out = (-MOTION_TV, 0, True)
            elif (nav_tower != None) and (queen == None):
                beh_out = beh.follow_nbr(nav_tower)
            elif neighbors.get_nbr_range_bits(queen) > 2:
                beh_out = beh.follow_nbr(queen, MOTION_TV)
            elif found_flower:
                state = STATE_RECRUIT
                start_time = sys.time()
            else:
                state = STATE_FOLLOW
                start_time = sys.time()

        elif state == STATE_FOLLOW:
            recruiter = find_recruiter()
            if recruiter == None:
                beh_out = beh.BEH_INACTIVE
                if sys.time() > (follow_start_time + FOLLOW_TIME):
                    wander()
            else:
                bearing = neighbors.get_nbr_bearing(recruiter)
                orientation = neighbors.get_nbr_orientation(recruiter)
                align_with(math.pi + bearing - orientation)

        elif state == STATE_GO:
            flower = detflower()
            if not flower == None:
                state = STATE_MOVE_TO_FLOWER
            beh_out = beh.tvrv(MOTION_TV, 0)

        elif state == STATE_RECRUIT:
            if sys.time() > (recruit_start_time + RECRUIT_TIME):
                align_with(pose.get_theta() - math.pi)

        elif state == STATE_ALIGN:
            tv = 0
            heading_error = math.normalize_angle(pose.get_theta() - target_theta)
            rv = ROTATE_RV_GAIN * heading_error
            beh_out = beh.tvrv(tv, rv)
            # you could actually do a running average in the list here
            small_error = hba.average_error_check(heading_error, [], HEADING_ERROR_LIMIT, new_nbrs)
            if new_nbrs:
                print "error", error_list
            if small_error:
                state = STATE_GO
        #END OF FINITE STATE MACHINE 

        bump_beh_out = beh.bump_beh(MOTION_TV)
        if state not in [STATE_RETURN_TO_BASE, STATE_COLLECT_POLLEN, STATE_RECRUIT]:
            beh_out = beh.subsume([beh_out, bump_beh_out])
        beh.motion_set(beh_out)
        hba.set_msg(state, my_color, 0)
コード例 #41
0
ファイル: summer.py プロジェクト: kgmstwo/THBCP
def summer():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()

        nbrList = neighbors.get_neighbors()
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE

        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_FIND_QUEEN
            if rone.button_get_value('b'):
                state = STATE_QUEEN
            if new_nbrs:
                print "idle"
        elif state == STATE_FIND_QUEEN:
            leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
            beh_out = beh.tvrv(MOTION_TV, 0)
            queen = get_queen()
            if not queen == None:
                state = STATE_BUMP_QUEEN
            else:
                #go straight and hope for the best
                beh_out = beh.tvrv(MOTION_TV, 0)  
        elif state == STATE_BUMP_QUEEN:
            leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
            queen = get_queen()
            if queen == None:
                state = STATE_RETURN
            else:
                if (neighbors.get_nbr_range_bits(queen) > 6) or (beh.bump_angle_get() != None):
                    state = STATE_BACK_UP
                    start_time = sys.time()
                else:
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
        elif state == STATE_BACK_UP:
            if sys.time() > start_time + BACK_UP_TIME:
                state = STATE_RETURN
            else:
                beh_out = beh.tvrv(-MOTION_TV, 0)
        elif state == STATE_RETURN:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_FIND_QUEEN
            queen = get_queen()
            if queen == None:
                state = STATE_IDLE
            else:
                beh_out = beh.avoid_nbr(queen, MOTION_TV)
        elif state == STATE_QUEEN:
            if new_nbrs:
                print 'Ich bin die Koenigin der welt!'

        # end of the FSM
        bump_beh_out = beh.bump_beh(MOTION_TV)
        if not state == STATE_QUEEN:
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        hba.set_msg(state, 0, 0)
コード例 #42
0
ファイル: spring.py プロジェクト: kgmstwo/THBCP
def spring():
    at_tree_odo = None
    tree_pose = None
    followers = 0
    have_seen_leader = False
    beh.init(0.22, 40, 0.5, 0.1)
    motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE, MOTION_RELEASE_DISTANCE
            , MOTION_CAPTURE_ANGLE, MOTION_RELEASE_ANGLE)
    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()
        nbr_list = neighbors.get_neighbors()
        if new_nbrs:
            print state
            
        beh_out = beh.BEH_INACTIVE

        # set colors, because why not do it at the top
        color_counts = [0, 0, 0]
        for i in range(3):
            color_counts[i] = min([5, (state - 5 * i)])
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
        elif state == STATE_SUCCESS:
            leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
        else:
            leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)

        if rone.button_get_value('g'):
            tree_pose = None
            followers = 0
            have_seen_leader = False
            state = STATE_IDLE
            
        # this is the main finite-state machine
        if not state in [STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW]:
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
                    start_time = sys.time()
                    state = STATE_FOLLOW
                    
        if state == STATE_IDLE:

            if rone.button_get_value('r'):
                pose.set_pose(0, 0, 0)
                state = STATE_WANDER
            elif rone.button_get_value('b'):
                state = STATE_QUEEN

        elif state == STATE_QUEEN:
            pass

        elif state == STATE_WANDER:
            ##            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            nav = hba.find_nav_tower_nbr(NAV_ID)
            beh_out = beh.avoid_nbr(nav, MOTION_TV)
            queen = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr

            if bump_front():
                if queen != None:
                    state = STATE_SUCCESS
                else:
                    tree_pose = pose.get_pose()
                    motion.set_goal((0.0, 0.0), MOTION_TV)
                    at_tree_odo = pose.get_odometer()
                    state = STATE_RETURN
            elif nav == None:
                motion.set_goal((0.0, 0.0), MOTION_TV)
                state = STATE_RETURN

        elif state == STATE_RETURN:
            ##            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = None
            recruiter = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr
                elif nbr_state == STATE_RECRUIT:
                    recruiter = nbr
            if queen != None:
                if (recruiter == None) and (tree_pose != None) and \
                close_to_nbr(queen):
                    start_time = sys.time()
                    dist_traveled = pose.get_odometer() - at_tree_odo
                    at_queen_odo = pose.get_odometer()
                    state = STATE_RECRUIT
                elif not closer_to_nbr(queen):
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
                else:
                    start_time = sys.time()
                    state = STATE_FOLLOW
            else:
                (tv, rv) = motion.update()
                beh_out = beh.tvrv(tv, rv)

        elif state == STATE_RECRUIT:
            new_followers = 0
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    if neighbors.get_nbr_id(nbr) > rone.get_id():
                        state = STATE_FOLLOW
                elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
                    new_followers += 1
            if new_followers > followers:
                print 'reset timer'
                start_time = sys.time()
            followers = max([followers, new_followers])
            if followers == 4 or sys.time() > start_time + WAIT_TIME:
                tree_pos = (tree_pose[0], tree_pose[1])
                motion.set_goal(tree_pos, MOTION_TV)
                state = STATE_LEADER

        elif state == STATE_FOLLOW: 
            recruiter = None
            leader = None
            success = False
            new_followers = 1
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    recruiter = nbr
                elif nbr_state == STATE_LEADER:
                    leader = nbr
                elif nbr_state == STATE_SUCCESS:
                    leader = nbr
                    success = True
                elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
                    new_followers += 1
            if success:
                have_seen_leader = True
            if new_followers > followers:
                start_time = sys.time()
            followers = max([followers, new_followers])

            if recruiter == None:
                if leader == None:
                    if have_seen_leader:
                        beh_out = beh.tvrv(MOTION_TV, 0)
                    elif followers == 5 or sys.time() > start_time + WAIT_TIME:
                        followers = 0
                        state = STATE_WANDER
                else:
                    if success and bump() and close_to_nbr(leader):
                        state = STATE_SUCCESS
                    beh_out = beh.follow_nbr(leader, MOTION_TV)
            
        elif state == STATE_LEADER:
##            (tv, rv) = motion.update()
##            beh_out = beh.tvrv(tv, rv)
##            if motion.is_done():
##                state = STATE_SUCCESS
            beh_out = beh.tvrv(-MOTION_TV, 0)

            if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
                state = STATE_SUCCESS

        elif state == STATE_SUCCESS:
            pass
        
        # end of the FSM
        
        if state not in [STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS, STATE_QUEEN]:
            bump_beh_out = beh.bump_beh(MOTION_TV)
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        msg = [0, 0, 0]
        msg[MSG_IDX_STATE] = state
        hba.set_msg(msg[0], msg[1], msg[2])
コード例 #43
0
ファイル: test.py プロジェクト: mts90/OpenPilot-1
#print(mb-ma)
#mb = sys.heap()
#mb = mb[0]
import flightplanstatus
#ma = sys.heap()
#ma = ma[0]
#print('import flightplanstatus')
#print(mb-ma)
#mb = sys.heap()
#mb = mb[0]
import mixersettings
#ma = sys.heap()
#ma = ma[0]
#print('import mixersettings')
#print(mb-ma)

n = 0
timenow = sys.time()
fpStatus = flightplanstatus.FlightPlanStatus()

while n < 120:
    n = n + 1
    #openpilot.debug(n, timenow)
    fpStatus.read()
    fpStatus.Debug.value[0] = n
    fpStatus.Debug.value[1] = timenow
    fpStatus.write()
    timenow = openpilot.delayUntil(timenow, 1000)
    if openpilot.hasStopRequest():
        sys.exit()
コード例 #44
0
ファイル: velocity.py プロジェクト: rdspring1/rone-testbed
def update():
    if sys.time() > _vcstate['update_time']:
        _vcstate['update_time'] += _VEL_UPDATE_PERIOD
        _velocity('l')
        _velocity('r')
コード例 #45
0
def init():
    _leds_state['color'] = 'r'
    _leds_state['pattern'] = 'circle'
    _leds_state['brightness'] = 10
    _leds_state['counter'] = 0
    _leds_state['update_time'] = sys.time()
コード例 #46
0
def waypoint_motion():
    velocity.init(0.22, 40, 0.5, 0.1)
    leds.init()
    poseX.init(pose_update)
    motionX.init(compute_goal_distance_and_heading, motion_controller_tv,
                 motion_controller_rv)

    pose_estimator_print_time = sys.time()
    mode = MODE_INACTIVE
    pose_old = (0.0, 0.0, 0.0)

    waypoint_list = []
    while True:
        # update the LED animations
        leds.update()

        # update the pose estimator
        poseX.update()

        # update the motion controller
        (tv, rv) = motionX.update()
        velocity.set_tvrv(tv, rv)

        # update the velocity controller if you are active, otherwise coast so the robot can be pushed
        if mode == MODE_ACTIVE:
            velocity.update()
        else:
            rone.motor_set_pwm('l', 0)
            rone.motor_set_pwm('r', 0)

        # print status every 500ms
        current_time = sys.time()
        if sys.time() > pose_estimator_print_time:
            pose_estimator_print_time += 250
            print 'goal', motionX.get_goal(), 'pose', poseX.get_pose(
            ), 'odo', poseX.get_odometer()
            if mode == MODE_INACTIVE:
                if (math2.pose_subtract(poseX.get_pose(), pose_old) !=
                    (0.0, 0.0, 0.0)):
                    # We're moving!  Yay!  Blink excitedly!
                    leds.set_pattern('r', 'blink_fast',
                                     int(LED_BRIGHTNESS * 1.5))
                else:
                    # not moving. sad face.
                    leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            pose_old = poseX.get_pose()

        # check the buttons.  If the red button is pressed, load the waypoint list
        if rone.button_get_value('r'):
            if mode == MODE_INACTIVE:
                poseX.set_pose(0, 0, 0)
                #waypoint_list = [(608, 0), (608, 304), (0, 304), (0, 0)]
                waypoint_list = [(700, 0), (700, 700), (0, 700), (0, 0)]
                mode = MODE_ACTIVE

        # check to see if you are at your waypoint.  If so, go to the next one
        if mode == MODE_ACTIVE:
            leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
            if motionX.is_done():
                ## Do we have another waypoint?
                if len(waypoint_list) > 0:
                    leds.set_pattern('rgb', 'group', LED_BRIGHTNESS)
                    sys.sleep(250)
                    waypoint = waypoint_list.pop(0)
                    print 'waypoint', waypoint
                    motionX.set_goal(waypoint, MOTION_TV)
                else:
                    print 'waypoint list empty'
                    mode = MODE_INACTIVE
                    velocity.set_tvrv(0, 0)
コード例 #47
0
def spring():
    at_tree_odo = None
    tree_pose = None
    followers = 0
    have_seen_leader = False
    beh.init(0.22, 40, 0.5, 0.1)
    motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE,
                   MOTION_RELEASE_DISTANCE, MOTION_CAPTURE_ANGLE,
                   MOTION_RELEASE_ANGLE)
    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()
        nbr_list = neighbors.get_neighbors()
        if new_nbrs:
            print state

        beh_out = beh.BEH_INACTIVE

        # set colors, because why not do it at the top
        color_counts = [0, 0, 0]
        for i in range(3):
            color_counts[i] = min([5, (state - 5 * i)])
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
        elif state == STATE_SUCCESS:
            leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
        else:
            leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)

        if rone.button_get_value('g'):
            tree_pose = None
            followers = 0
            have_seen_leader = False
            state = STATE_IDLE

        # this is the main finite-state machine
        if not state in [
                STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW
        ]:
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
                    start_time = sys.time()
                    state = STATE_FOLLOW

        if state == STATE_IDLE:

            if rone.button_get_value('r'):
                pose.set_pose(0, 0, 0)
                state = STATE_WANDER
            elif rone.button_get_value('b'):
                state = STATE_QUEEN

        elif state == STATE_QUEEN:
            pass

        elif state == STATE_WANDER:
            ##            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            nav = hba.find_nav_tower_nbr(NAV_ID)
            beh_out = beh.avoid_nbr(nav, MOTION_TV)
            queen = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr

            if bump_front():
                if queen != None:
                    state = STATE_SUCCESS
                else:
                    tree_pose = pose.get_pose()
                    motion.set_goal((0.0, 0.0), MOTION_TV)
                    at_tree_odo = pose.get_odometer()
                    state = STATE_RETURN
            elif nav == None:
                motion.set_goal((0.0, 0.0), MOTION_TV)
                state = STATE_RETURN

        elif state == STATE_RETURN:
            ##            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = None
            recruiter = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr
                elif nbr_state == STATE_RECRUIT:
                    recruiter = nbr
            if queen != None:
                if (recruiter == None) and (tree_pose != None) and \
                close_to_nbr(queen):
                    start_time = sys.time()
                    dist_traveled = pose.get_odometer() - at_tree_odo
                    at_queen_odo = pose.get_odometer()
                    state = STATE_RECRUIT
                elif not closer_to_nbr(queen):
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
                else:
                    start_time = sys.time()
                    state = STATE_FOLLOW
            else:
                (tv, rv) = motion.update()
                beh_out = beh.tvrv(tv, rv)

        elif state == STATE_RECRUIT:
            new_followers = 0
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    if neighbors.get_nbr_id(nbr) > rone.get_id():
                        state = STATE_FOLLOW
                elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
                    new_followers += 1
            if new_followers > followers:
                print 'reset timer'
                start_time = sys.time()
            followers = max([followers, new_followers])
            if followers == 4 or sys.time() > start_time + WAIT_TIME:
                tree_pos = (tree_pose[0], tree_pose[1])
                motion.set_goal(tree_pos, MOTION_TV)
                state = STATE_LEADER

        elif state == STATE_FOLLOW:
            recruiter = None
            leader = None
            success = False
            new_followers = 1
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    recruiter = nbr
                elif nbr_state == STATE_LEADER:
                    leader = nbr
                elif nbr_state == STATE_SUCCESS:
                    leader = nbr
                    success = True
                elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
                    new_followers += 1
            if success:
                have_seen_leader = True
            if new_followers > followers:
                start_time = sys.time()
            followers = max([followers, new_followers])

            if recruiter == None:
                if leader == None:
                    if have_seen_leader:
                        beh_out = beh.tvrv(MOTION_TV, 0)
                    elif followers == 5 or sys.time() > start_time + WAIT_TIME:
                        followers = 0
                        state = STATE_WANDER
                else:
                    if success and bump() and close_to_nbr(leader):
                        state = STATE_SUCCESS
                    beh_out = beh.follow_nbr(leader, MOTION_TV)

        elif state == STATE_LEADER:
            ##            (tv, rv) = motion.update()
            ##            beh_out = beh.tvrv(tv, rv)
            ##            if motion.is_done():
            ##                state = STATE_SUCCESS
            beh_out = beh.tvrv(-MOTION_TV, 0)

            if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
                state = STATE_SUCCESS

        elif state == STATE_SUCCESS:
            pass

        # end of the FSM

        if state not in [
                STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS,
                STATE_QUEEN
        ]:
            bump_beh_out = beh.bump_beh(MOTION_TV)
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        msg = [0, 0, 0]
        msg[MSG_IDX_STATE] = state
        hba.set_msg(msg[0], msg[1], msg[2])
コード例 #48
0
def summer():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()

        nbrList = neighbors.get_neighbors()
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE

        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_FIND_QUEEN
            if rone.button_get_value('b'):
                state = STATE_QUEEN
            if new_nbrs:
                print "idle"
        elif state == STATE_FIND_QUEEN:
            leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
            beh_out = beh.tvrv(MOTION_TV, 0)
            queen = get_queen()
            if not queen == None:
                state = STATE_BUMP_QUEEN
            else:
                #go straight and hope for the best
                beh_out = beh.tvrv(MOTION_TV, 0)
        elif state == STATE_BUMP_QUEEN:
            leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
            queen = get_queen()
            if queen == None:
                state = STATE_RETURN
            else:
                if (neighbors.get_nbr_range_bits(queen) >
                        6) or (beh.bump_angle_get() != None):
                    state = STATE_BACK_UP
                    start_time = sys.time()
                else:
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
        elif state == STATE_BACK_UP:
            if sys.time() > start_time + BACK_UP_TIME:
                state = STATE_RETURN
            else:
                beh_out = beh.tvrv(-MOTION_TV, 0)
        elif state == STATE_RETURN:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_FIND_QUEEN
            queen = get_queen()
            if queen == None:
                state = STATE_IDLE
            else:
                beh_out = beh.avoid_nbr(queen, MOTION_TV)
        elif state == STATE_QUEEN:
            if new_nbrs:
                print 'Ich bin die Koenigin der welt!'

        # end of the FSM
        bump_beh_out = beh.bump_beh(MOTION_TV)
        if not state == STATE_QUEEN:
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        hba.set_msg(state, 0, 0)
コード例 #49
0
ファイル: hba.py プロジェクト: va17/THBCP
def winter_time_keeper(initial_time):
    score_time = sys.time() - initial_time
    return score_time
コード例 #50
0
ファイル: openpilot.py プロジェクト: mcu786/my_OpenPilot_mods
def time():
	return sys.time()