class FidoTail(FSMClient):
def __init__(self, inputs, outputs):
self.inputs = inputs
self.outputs = outputs
self.fsm = Fysom({'initial': 'Idle',
'events': [
{'name': 'start_wagging', 'src': ['Idle', 'Stopping'], 'dst': 'StartingWagging'},
{'name': 'started', 'src': 'StartingWagging', 'dst': 'WaggingRight'},
{'name': 'timeout', 'src': 'WaggingLeft', 'dst': 'WaggingRight'},
{'name': 'timeout', 'src': 'WaggingRight', 'dst': 'WaggingLeft'},
{'name': 'stop_wagging', 'src': ['WaggingLeft', 'WaggingRight'], 'dst': 'Stopping'},
{'name': 'stop_wagging', 'src': 'Idle', 'dst': 'Idle'},
{'name': 'stopped', 'src': 'Stopping', 'dst': 'Idle'}],
'callbacks': {
'onenterStartingWagging': self._enterStartingWagging,
'onenterWaggingRight': self._enterWaggingRight,
'doWaggingRight': self._doWaggingRight,
'onenterWaggingLeft': self._enterWaggingLeft,
'doWaggingLeft': self._doWaggingLeft,
'onenterStopping': self._enterStopping}})
#
# fsm action methods (private)
#
def _enterStartingWagging(self, event):
self.delay = event.delay
self.count = event.count
self.fsm.started()
def _enterWaggingRight(self, event):
#servo_if.set_servo_reliably(servo_if.TAIL, servo_if.TAIL_RIGHT)
self.outputs['tail'] = servo_if.TAIL_RIGHT
self.timeout_time = time.time() + self.delay
def _doWaggingRight(self):
if time.time() >= self.timeout_time:
self.fsm.timeout()
def _enterWaggingLeft(self, event):
#servo_if.set_servo_reliably(servo_if.TAIL, servo_if.TAIL_LEFT)
self.outputs['tail'] = servo_if.TAIL_LEFT
self.timeout_time = time.time() + self.delay
def _doWaggingLeft(self):
if time.time() >= self.timeout_time:
self.count -= 1
if self.count > 0:
self.fsm.timeout()
else:
self.fsm.stop_wagging()
def _enterStopping(self, event):
#servo_if.set_servo(servo_if.TAIL, servo_if.TAIL_CENTER)
self.outputs['tail'] = servo_if.TAIL_CENTER
self.fsm.stopped()
#
# public interface methods
#
def start_wagging(self, delay=0.25, count=5):
self.fsm.start_wagging(delay=delay, count=count)
def stop_wagging(self):
self.fsm.stop_wagging()
def is_wagging(self):
return self.fsm.current != 'Idle'
