# This implements example AFSM that:
# 1. Drives robot with obstacle avoidance using sonar ranger device
# 2. Controls gripper according to blobfinder readings:
#    - if any blue blob is found, gripper is opened
#    - if any red blob is found, gripper is closed
#
# To start it, run Stage with everything.cfg world (you can also use
# real robot) then start one more Player instance at port 8000 with such
# configuration file:
#
#  driver
#  (
#    name "sonartoranger"
#    provides ["ranger:0"]
#    requires ["6665:sonar:0"]
#    alwayson 0
#  )
#
# Let's name it ranger.cfg, you can start it by typing:
#    player -p 8000 ranger.cfg
# Then you can start Player instance with this controller,
# (for example on port 7000) by typing:
#    player -p 7000 afsm.cfg
#
# Figure in afsm.eps file depicts AFSM implemented here.

driver
(
  name "passthrough"
  provides "position2d:0"
  requires "6665:position2d:0"
)
driver
(
  name "passthrough"
  provides ["ranger:0"]
  requires ["8000:ranger:0"]
)
driver
(
  name "passthrough"
  provides ["gripper:0"]
  requires ["6665:gripper:0"]
)
driver
(
  name "passthrough"
  provides ["blobfinder:0"]
  requires ["6665:blobfinder:0"]
)

#### motors operation:

driver
(
  name "suppressor"
  provides ["master:::position2d:10" "slave:::position2d:11"]
  requires ["position2d:0"]
  fadeout_time 0.333
)
driver
(
  name "suppressor"
  provides ["master:::position2d:20" "slave:::position2d:21"]
  requires ["position2d:10"]
  fadeout_time 0.333
)
driver
(
  name "suppressor"
  provides ["master:::position2d:30" "slave:::position2d:31"]
  requires ["position2d:11"]
  fadeout_time 0.333
)
driver
(
  name "inhibitor"
  provides ["switch:::dio:10" "comm:::position2d:40"]
  requires ["comm:::position2d:21"]
  bitmask "1"
)
driver
(
  name "inhibitor"
  provides ["switch:::dio:20" "comm:::position2d:50"]
  requires ["comm:::position2d:30"]
  bitmask "1"
)
driver
(
  name "inhibitor"
  provides ["switch:::dio:30" "comm:::position2d:60"]
  requires ["comm:::position2d:20"]
  bitmask "1"
)
driver
(
  name "cmdsplitter"
  provides ["position2d:70"]
  devices 2
  requires ["0:::position2d:50" "1:::position2d:60"]
)
driver
(
  name "diodelay"
  provides ["dio:100"]
  requires ["state:::dio:30"]
  init_state "0"
  wait_on_1 1.5
  wait_on_0 0.8
  fade_out 0.7
)
driver
(
# go
  name "velcmd"
  provides ["opaque:0"]
  requires ["position2d:31" "ranger:0"]
  px 3.0
  py 0.0
  pa 0.0
  pxr 0.2
  ranger_power_on 0
  first_idx 2
  last_idx 5
  alwayson 1
)
driver
(
# turn left
  name "velcmd"
  provides ["opaque:1"]
  requires ["position2d:70"]
  px 0.0
  py 0.0
  pa 1.0
  alwayson 1
)
driver
(
# turn right
  name "velcmd"
  provides ["opaque:2"]
  requires ["position2d:40"]
  px 0.0
  py 0.0
  pa -1.0
  alwayson 1
)
driver
(
  name "bitlogic"
  function "or"
  slots 2
  provides ["0:::dio:40" "1:::dio:41"]
  requires ["dio:100"]
  init_bits "0"
)
driver
(
  name "cmdsplitter"
  provides ["dio:50"]
  devices 2
  requires ["0:::dio:10" "1:::dio:40"]
)
driver
(
  name "cmdsplitter"
  provides ["dio:51"]
  devices 2
  requires ["0:::dio:20" "1:::dio:41"]
)
driver
(
  name "rangertodio"
  provides ["dio:0"]
  requires ["ranger:0" "dio:50"]
  start_idx 1
  end_idx 3
  bits 1
  threshold 0.9
  turn_power_on 0
  alwayson 1
)
driver
(
  name "rangertodio"
  provides ["dio:1"]
  requires ["ranger:0" "dio:51"]
  start_idx 4
  end_idx 6
  bits 1
  threshold 0.9
  turn_power_on 0
  alwayson 1
)

#### gripper operation:

driver
(
  name "suppressor"
  provides ["master:::gripper:10" "slave:::gripper:11"]
  requires ["gripper:0"]
  fadeout_time 0.333
)
driver
(
  name "inhibitor"
  provides ["switch:::dio:110" "comm:::gripper:20"]
  requires ["comm:::gripper:10" "switch:::dio:130"]
  bitmask "01"
  neg 1
)
driver
(
  name "inhibitor"
  provides ["switch:::dio:111" "comm:::gripper:21"]
  requires ["comm:::gripper:20"]
  bitmask "01"
)
driver
(
  name "inhibitor"
  provides ["switch:::dio:120" "comm:::gripper:30"]
  requires ["comm:::gripper:11" "switch:::dio:130"]
  bitmask "10"
  neg 1
)
driver
(
  name "inhibitor"
  provides ["switch:::dio:121" "comm:::gripper:31"]
  requires ["comm:::gripper:30"]
  bitmask "10"
)
driver
(
  name "gripcmd"
  provides ["opaque:10"]
  requires ["gripper:21"]
  cmd "close"
  alwayson 1
)
driver
(
  name "gripcmd"
  provides ["opaque:11"]
  requires ["gripper:31"]
  cmd "open"
  alwayson 1
)
driver
(
  name "diodelay"
  provides ["dio:130"]
  init_state "00"
  wait_on_1 4.0
  wait_on_0 0.0
  fade_out 0.0
)
driver
(
  name "cmdsplitter"
  provides ["dio:140"]
  devices 3
  requires ["0:::dio:111" "1:::dio:121" "2:::dio:130"]
)
driver
(
  name "blobtodio"
  provides ["dio:150"]
  requires ["blobfinder:0" "dio:140"]
# red and blue:
  color[0] [255 0 0]
  color[1] [0 0 255]
  alwayson 1
)