class FidoBrain(FSMClient):
def __init__(self, inputs, outputs):
self.inputs = inputs
self.outputs = outputs
self.fsm = Fysom({'initial': 'SeekingBall',
'events': [
{'name': 'found_ball', 'src': 'SeekingBall', 'dst': 'TrackingBall'},
{'name': 'lost_ball', 'src': ['TrackingBall', 'ApproachingBall'], 'dst': 'SeekingBall'},
{'name': 'ball_on_ground', 'src': 'TrackingBall', 'dst': 'ApproachingBall'},
{'name': 'ball_above_ground', 'src': 'ApproachingBall', 'dst': 'TrackingBall'},
{'name': 'near_ball', 'src': 'ApproachingBall', 'dst': 'FinalApproach'},
{'name': 'at_ball', 'src': 'FinalApproach', 'dst': 'PickingUpBall'},
{'name': 'have_ball', 'src': 'PickingUpBall', 'dst': 'TurningAround'},
{'name': 'turned_around', 'src': 'TurningAround', 'dst': 'SeekingFace'},
{'name': 'check_for_face', 'src': 'SeekingFace', 'dst': 'CheckingForFace'},
{'name': 'no_face', 'src': 'CheckingForFace', 'dst': 'SeekingFace'},
{'name': 'found_face', 'src': 'CheckingForFace', 'dst': 'ApproachingFace'},
{'name': 'lost_face', 'src': 'ApproachingFace', 'dst': 'SeekingFace'},
{'name': 'at_face', 'src': ['SeekingFace', 'ApproachingFace'], 'dst': 'DroppingBall'},
{'name': 'dropped_ball', 'src': 'DroppingBall', 'dst': 'SeekingBall'},
{'name': 'posed_for_play', 'src': 'PosingForPlay', 'dst': 'SeekingBall'}],
'callbacks': {
'onenterSeekingBall': self._enterSeekingBall,
'doSeekingBall': self._doSeekingBall,
'onenterTrackingBall': self._enterTrackingBall,
'doTrackingBall': self._doTrackingBall,
'onenterApproachingBall': self._enterApproachingBall,
'doApproachingBall': self._doApproachingBall,
'onenterFinalApproach': self._enterFinalApproach,
'doFinalApproach': self._doFinalApproach,
'onenterPickingUpBall': self._enterPickingUpBall,
'doPickingUpBall': self._doPickingUpBall,
'onenterTurningAround': self._enterTurningAround,
'doTurningAround': self._doTurningAround,
'onenterSeekingFace': self._enterSeekingFace,
'doSeekingFace': self._doSeekingFace,
'onenterCheckingForFace': self._enterCheckingForFace,
'doCheckingForFace': self._doCheckingForFace,
'onenterApproachingFace': self._enterApproachingFace,
'doApproachingFace': self._doApproachingFace,
'onenterDroppingBall': self._enterDroppingBall,
'doDroppingBall': self._doDroppingBall,
'onenterPosingForPlay': self._enterPosingForPlay,
'doPosingForPlay': self._doPosingForPlay}})
def should_find_ball(self):
return self.fsm.current in ['SeekingBall', 'TrackingBall', 'ApproachingBall', 'FinalApproach']
def should_find_face(self):
return self.fsm.current in ['SeekingFace', 'CheckingForFace', 'ApproachingFace']
#
# fsm action methods (private)
#
def _enterSeekingBall(self, event):
pass
def _doSeekingBall(self):
self.outputs['left_wheel'] = -100
self.outputs['right_wheel'] = 100
self.outputs['legs'] = servo_if.LEGS_UP
self.outputs['neck'] = servo_if.NECK_START
self.outputs['head'] = servo_if.HEAD_CENTER
if self.inputs['ball_area'] > 50:
self.fsm.found_ball()
def _enterTrackingBall(self, event):
pass
def _doTrackingBall(self):
#if we lose the ball, go back to seeking; if it's on the ground, approach it.
if self.inputs['ball_area'] <= 40:
self.fsm.lost_ball()
elif self.inputs['ball_area'] >= 50 and self.inputs['ball_y'] >= 120 and self.outputs['neck'] < servo_if.NECK_CENTER:
self.fsm.ball_on_ground()
elif random.random() > 0.95:
self.outputs['jaw'] = (servo_if.JAW_OPEN + servo_if.JAW_CLOSED_EMPTY) / 2
play_sound("bark03.mp3")
def _enterApproachingBall(self, event):
self.outputs['legs'] = servo_if.LEGS_UP
def _doApproachingBall(self):
if self.inputs['ball_area'] <= 40:
self.fsm.lost_ball()
elif self.inputs['ball_y'] < 240 and self.outputs['neck'] > servo_if.NECK_CENTER:
self.fsm.ball_above_ground()
elif self.inputs['avg_ball_area'] > NEAR_BALL_AREA:
#self.outputs['left_wheel'] = 0
#self.outputs['right_wheel'] = 0
self.fsm.near_ball()
def _enterFinalApproach(self, event):
self.last_ir_l = self.inputs['ir_l']
self.last_ir_m = self.inputs['ir_m']
self.last_ir_r = self.inputs['ir_r']
def _doFinalApproach(self):
ir_l = self.inputs['ir_l']
ir_m = self.inputs['ir_m']
ir_r = self.inputs['ir_r']
fast_approach_speed = 140
slow_approach_speed = 80
rotate_speed = 70
ir_detect_dist = 60
if (ir_l < 60 and ir_m < 60) or (ir_m < 60 and ir_r < 60):
# at least one IR sensor sees the ball; center it using the 3 IR sensors
self.last_ir_l = ir_l
self.last_ir_m = ir_m
self.last_ir_r = ir_r
if ir_m < ir_l and ir_m < ir_r:
# centered; continue forward until it's at the right distance
if ir_m > 20:
# approach ball fast
self.outputs['left_wheel'] = fast_approach_speed
self.outputs['right_wheel'] = fast_approach_speed
elif ir_m > 11:
# approach ball
self.outputs['left_wheel'] = slow_approach_speed
self.outputs['right_wheel'] = slow_approach_speed
elif ir_m < 10:
# too close, back up!
self.outputs['left_wheel'] = -slow_approach_speed
self.outputs['right_wheel'] = -slow_approach_speed
else:
# close enough!
self.outputs['left_wheel'] = 0
self.outputs['right_wheel'] = 0
self.fsm.at_ball()
elif ir_l < 11 or ir_r < 11:
# too close to obstacle, back up!
self.outputs['left_wheel'] = -fast_approach_speed
self.outputs['right_wheel'] = -fast_approach_speed
elif ir_l < ir_r:
# rotate left
self.outputs['left_wheel'] = -rotate_speed
self.outputs['right_wheel'] = rotate_speed
else:
# rotate right
self.outputs['left_wheel'] = rotate_speed
self.outputs['right_wheel'] = -rotate_speed
else:
# no ir sensor sees ball; rotate in direction of IR that last saw it
if self.last_ir_l < self.last_ir_r:
# rotate left
self.outputs['left_wheel'] = -rotate_speed
self.outputs['right_wheel'] = rotate_speed
else:
# rotate right
self.outputs['left_wheel'] = rotate_speed
self.outputs['right_wheel'] = -rotate_speed
def _enterPickingUpBall(self, event):
self.outputs['left_wheel'] = 0
self.outputs['right_wheel'] = 0
self.pickup_timer = time.time() + 0.5
def _doPickingUpBall(self):
if time.time() >= self.pickup_timer:
servo_if.grab_ball()
self.fsm.have_ball()
def _enterTurningAround(self, event):
self.turn_timer = time.time() + 1.0
def _doTurningAround(self):
turn_speed = 250
self.outputs['left_wheel'] = -turn_speed
self.outputs['right_wheel'] = turn_speed
if time.time() >= self.turn_timer:
self.fsm.turned_around()
def _enterSeekingFace(self, event):
self.outputs['head'] = servo_if.HEAD_CENTER
self.outputs['neck'] = servo_if.NECK_CENTER + 10
servo_if.ramp_servo(servo_if.NECK, servo_if.NECK_CENTER + 10, 3)
self.outputs['left_wheel'] = -50
self.outputs['right_wheel'] = 50
self.seekingFace_timer = time.time() + 0.2
def _doSeekingFace(self):
#self.outputs['head'] = servo_if.HEAD_CENTER
#self.outputs['neck'] = servo_if.NECK_UP + 1 if self.outputs['neck'] < servo_if.NECK_UP else servo_if.NECK_UP - 1
#self.outputs['left_wheel'] = -50
#self.outputs['right_wheel'] = 50
if time.time() >= self.seekingFace_timer:
self.fsm.check_for_face()
def _enterCheckingForFace(self, event):
self.outputs['left_wheel'] = 0
self.outputs['right_wheel'] = 0
self.checkingFace_timer = time.time() + 1.0
def _doCheckingForFace(self):
if self.inputs['face_area'] > 0:
self.fsm.found_face()
elif time.time() >= self.checkingFace_timer:
self.fsm.no_face()
def _enterApproachingFace(self, event):
self.lost_face_timer = 0
pass
def _doApproachingFace(self):
if self.inputs['face_area'] <= 0:
self.lost_face_timer = self.lost_face_timer + 1
if self.lost_face_timer > 2:
self.fsm.lost_face()
self.outputs['left_wheel'] = 0
self.outputs['right_wheel'] = 0
else:
self.lost_face_timer = 0
if self.inputs['face_area'] > NEAR_FACE_AREA:
self.fsm.at_face()
def _enterDroppingBall(self, event):
self.outputs['left_wheel'] = 0
self.outputs['right_wheel'] = 0
def _doDroppingBall(self):
servo_if.throw_ball()
self.fsm.dropped_ball()
def _enterPosingForPlay(self, event):
servo_if.pose_for_play()
self.outputs['legs'] = servo_if.LEGS_DOWN
self.pose_timer = time.time() + 2.0
def _doPosingForPlay(self):
if time.time() >= self.pose_timer:
self.outputs['legs'] = servo_if.LEGS_UP
self.fsm.posed_for_play()
