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)
def initialization(t, state):
state.wnd_main = "Blink Detection"
state.wnd_debug = "Diff VC"
state.streaming = True
state.debug = False
state.init_stage = False
state.tracking_stage = False
state.usage_text = "'s' Start - 'r' Reset - 'q' Quit"
state.locating_text = "Mode : Locating eye..."
state.tracking_text = "Mode : Tracking eye..."
state.blinked_text = "*Blinked*"
state.prev = None
state.diff = None
state.tpl = ()
state.comps = ()
state.blink = ()
state.color = (0, 255, 0)
state.diff_color = (255, 255, 0)
state.text_color = (0, 0, 255)
state.font = cv2.FONT_HERSHEY_PLAIN
state.delay = 0
# Initialize VideoCapture
state.vc, state.kernel = motion.init(state.wnd_main)
usage_text = "'s' Start - 'r' Reset - 'q' Quit" locating_text = "Mode : Locating eye..." tracking_text = "Mode : Tracking eye..." blinked_text = "*Blinked*" prev = None diff = None tpl = () comps = () blink = () color = (0,255,0) diff_color = (255,255,0) text_color = (0,0,255) font = cv2.FONT_HERSHEY_PLAIN delay = 0 # Initialize VideoCapture vc,kernel = motion.init(wnd_main) while streaming: # Register sequenced images and find all connected components key,frame = vc.read() # Write usage text cv2.putText(frame, usage_text, (20,20), font, 1.0, text_color) if init_stage: diff,contours = motion.get_components(frame, prev, kernel) comps = motion.get_moved_components(contours, 5, 5) # If not entering tracking_stage yet, try find eyes within contours if not contours == None and not tracking_stage: cv2.putText(frame, locating_text, (20,220), font, 1.0, text_color) tracking_stage,tpl = is_eyes(comps, frame) # Get ROI from eye template against current frame if tracking_stage: cv2.putText(frame, tracking_text, (20,220), font, 1.0, color)
def init(kff, kff_offset, kp, ki):
velocity.init(kff, kff_offset, kp, ki)
leds.init()
pose.init()
motion.init()
neighbors.init(NBR_PERIOD)
def init(kff, kff_offset, kp, ki):
velocity.init(kff, kff_offset, kp, ki)
leds.init()
pose.init()
motion.init()
neighbors.init(NBR_PERIOD)
