#Example of Behavior Based control with three simple behaviors.
#
# Jay Summet
# September 2015, released to the public domain

from Myro import *
init()

#Move behavior: Always recommends that the robot moves forward at 1/2 speed.
def move():
   return ( True, 0.5, 0.5)

#Avoid behavior: Turns if we detect something in front of us.
# (Otherwise, does not make a suggestions)
def avoid():
   v = getObstacle("center")
   if v > 0:
      return (True, -1, 1)
   else:
      return (False, 0,0 )

#SeekLight behavior: If we see a bright light, back towards it slowly.
def seekLight():
   l,c,r = getLight()
   if l > 62000 and r > 62000 and c > 62000:
      return (False, 0,0)
   if c < 62000:
      return (True, -0.25, -0.25)
   if l < 62000:
      return (True, -0.25, 0.25)
   if r < 62000:
      return (True, 0.25, -0.25)



#Arbitrate....call each function in turn, in the order they are listed in
# the actions list. If the function makes a recommendation, we do it, 
# otherwise we move on to the next behavior. NOTE: Move will ALWAYS
# recommend that we move forward, so that is our default behavior if
# the other two do not make a recommendation.
#
def arbitrate():
   actions = [ avoid, seekLight, move]

   for action in actions:
      doIt, l,r = action()
      if doIt == True:
         motors(l,r)
         return	 #If we execute a recommendation, quit the function.

#Run the behaviors for 30 seconds.
for t in timer(30):
   arbitrate()
stop()