Jimshidi, Chapter14, Mine Detector

;; ASE6102 System of Systems
;; Swarm Robotics - Demonstration Model
;; Based on system described in Jamshidi, Ch 14, 1st ed.
;; Team6


;;-----------------------------------
;; declarations
;;-----------------------------------

__includes ;; Moved to end of code since includes do not work with commons
[ ;;"messaging.nls"     ; handles agent messaging
  ;;"sxl-utils.nls"     ; small set of NetLogo utilities
]

globals
[ mine-active-color
  mine-deactive-color
  target_X                       ; xcoord of target mine
  target_Y                       ; ycoord of target mine
  swarmTeamSize                  ; number of robots in a swarm team
  disarm_Count                   ; count for number of disarming swarmRobots
  target_mine                    ; name of mine to help with
  leader                         ; lead swarmRobot (he who(m?) found the mine)
  closestSwarmMembers
]

frags-own [
  dim             ; used for fading particles
]

;directed-link-breed [red-links red-link]

;;-----------------------------------
;; breed data
;;-----------------------------------
breed [sensors sensor]
sensors-own
[ status          ;; off/monitoring/detecting
  name
]

breed [swarmRobots swarmRobot]
swarmRobots-own
[ status          ;; foraging/following_scent/waiting/homing/disarming
  target          ;; target mine
  leadSwarmBot    ;; lead swarmRobot
  start-time
  time-taken
  name
]

breed [baseRobots baseRobot]
baseRobots-own
[ status          ;; on/tx
  name
]

breed [mines mine]
mines-own
[ status          ;; hidden/found/disarmed[die]
  start-time
  time-taken
  name
]

breed [dynamicTargets dyanmicTarget]
dynamicTargets-own
[status
 start-time
 time-taken
 name
]

breed [obstacles obstacle]
obstacles-own
[ ob-dia
  ob-color
]

breed [ frags frag ]

turtles-own [
  col             ; sets color of an explosion particle
  x-vel           ; x-velocity
  y-vel           ; y-velocity
  #msgQ          ;; the message queue
  #msg-id        ;; the agent name/id for messaging
]

;;-----------------------------------
;; general
;;-----------------------------------

to setup-turtles
  ask turtles[
    set col 0
    set color 2
  ]
end 

to setup
  clear-all
  setup-globals
  setup-patches
  setup-obsticals
  setup-mines
  setup-swarmRobots
  setup-baseRobots
  setup-sensors
  setup-dynamicTargets
  reset-ticks
end 

to setup-globals
  ;; set globals
  set mine-active-color red
  set mine-deactive-color green
  set swarmTeamSize swarm-teamSize - 1
end 

to setup-patches
  ask patches [set pcolor gray]
end 

to setup-mines
  create-mines no-of-mines
  ask mines
  [ set name (word "mine_" who)
    setxy random-xcor random-ycor
    ;setxy 0 10
    set color black
    set shape "ufo top"
    set status "hidden"
    ask mines
    [
    ifelse (any? obstacles-on patch-here)
      [
        wiggle
      ][
        fd 2
      ]
    ]
  ]
end 

to setup-swarmRobots
  create-swarmRobots no-of-swarmRobots
  ask swarmRobots
  [ set name (word "swarmrobot " who)
    ; make-msgQ (word "swarm_" who)
    setxy 0 0
    set color blue
    set status "foraging"
  ]
end 

to setup-baseRobots
  create-baseRobots no-of-baseRobots
  ask baseRobots
  [ set name (word "base_" who)
    make-msgQ (word "base_" who)
    setxy 0 0
    set color green
    set size 1.5
    ;;set shape "square"
    set status "ON"
  ]
end 

to setup-sensors
  create-sensors 10
  [
    set shape "star"
    set size .5
    set color yellow
    let $n 0  ;; actual number of turtles on this patch
    let $s 0  ;; current number of turtles on a side of square pattern - 1
    set heading 0
    ask patch-here []
    repeat no-of-sensors   ;; number of needed turtles
    [ hatch 1 jump Grid   ;; make one turtle and move
      set $n $n + 1   ;; increment count of curent side
      ask patch-here []
      if $n > $s  ;; if side finished...
      [
        right 90 set $n 0   ;; turn and reset count
        ask patch-here []
        ;; if headed up or down, increment side count
        if heading mod 180 = 0 [ set $s $s + 1
          ]
      ]
    ]
   die
  ]
end 

to setup-dynamicTargets
  create-dynamicTargets no-of-dynamicTargets
  ask dynamicTargets
  [
    setxy random-xcor 20
    set shape "person"
    set size 1
    set color black
  ]
end 

to setup-obsticals
  create-obstacles no-of-obstacles
  ask obstacles
  [
    setxy random-xcor random-ycor
    set shape "square"
    set size random 5
    set col one-of base-colors
  ]
end 

to go
  if ticks > Game_Time_Out [ setup go ]   ;; stop when ticks run high
  if not any? mines [ setup go ]        ;; stop when no mines
  if count swarmRobots < swarm-teamSize [ setup go ] ;; cant disarm mines anymore
  forage-SwarmRobot
  homming
  comms-baseRobots
  following-scent
  ;obs-avoidance
  disarming
  timeOut
  blowup
  ask frags
  [
        fade
  ]
  every reset_Time
  [
    setup
    go
  ]
  tick
end 

to moveForage
  ask swarmRobots
  [
    if status = "foraging"
    [
      wiggle
      forward .1
      ; obs-avoidance
    ]
  ]
end 

to forage-SwarmRobot
  ask swarmRobots
  [
    if status = "foraging"
    [
      ;; look for any mines using the robots sensor
      ifelse any? mines in-cone sensor-range sensor-FoV  ;; [in-cone range fov_angle]
      [
        ;; check to see of the mine is already found and being disarmed
        ask nearest-of mines
        [
          ifelse status != "found"
          [
            set status "found"
            set start-time ticks
            set color red
            ;; update the status of the robot that found the mine
            ask nearest-of swarmRobots
            [
              ;; update status of swarmRobot that has found a mine
              set status "waiting"
              set color yellow
              set target nearest-of mines
              ;;fd 1 ; move forward one... this should be calculated
              face nearest-of mines

              ;; send message to baseRobot with x,y coordinates
              broadcast baseRobots (list ([xcor] of nearest-of mines) ([ycor] of nearest-of mines) nearest-of swarmRobots who)
              ;;ask nearest-of swarmRobots [
                ;create-red-link-to nearest-of baseRobots
                ;show link-with nearest-of baseRobots
              ;]
            ]
          ]
          [
            ;; tell any other robots to move along if they are not waiting
            ;; if they see the mine
            ask swarmRobots
            [
              if status != "waiting" [moveForage]
            ]

          ]
        ]
      ]
      ;; else keep foraging for other mines
      [
        moveForage
      ]
    ]
  ]
end 

to comms-baseRobots
  ;; base robots get told about mines to disarm by swarmRobot and sends helper swarmRobots
  ask baseRobots
    [ if msg-waiting? ;; check for a message [Message = swarmRobot#(mineX, mineY)
      [
        ;; added a queue hold for next bot if two mines are detected at the same time with
        ;; less than the required bots foraging to send to disarm
        ifelse count swarmRobots with [status = "foraging"] < swarm-teamSize - 1
        [
          set color yellow
          if swarmRobots with [status = "scent-following"] != nobody
          [
          set closestSwarmMembers up-to-n-of (swarm-teamSize - 1) swarmRobots with [status = "following_scent"]
          ;show closestSwarmMembers
          ask closestSwarmMembers ;up-to-n-of (swarm-teamSize - 1) swarmRobots with [status = "foraging"]
            [
              right 180
              fd 1
              set status "foraging"
              set color blue
            ]
          ]

        ;; do nothing set
        ]
        [
            set color black
            set status "tx"
            let m get-msg ;; fetch message - clears queue
            let from item 0 m ;; mineX
            let msg  item 1 m ;; mineY
            set target_X   item 0 msg ;; move value to var
            set target_Y   item 1 msg ;; move value to var
            set leader     item 2 msg ;; lead swarm robot name

            ;; set the target mine variable to the mine name
            set target_mine one-of mines with
                  [(xcor = [target_X] of myself) and (ycor = [target_Y] of myself)]

            ;; now ask some others of the swarm to go help with the disarming of the mine
            ;; right now this is random but a good upgrade would be to grab the closest avalible
          set closestSwarmMembers up-to-n-of (swarm-teamSize - 1) swarmRobots with [status = "foraging"]
          ;show closestSwarmMembers
          ask closestSwarmMembers ;up-to-n-of (swarm-teamSize - 1) swarmRobots with [status = "foraging"]
            [
              set status "following_scent"
              set start-time ticks
              set color orange
              set target target_mine
              set leadSwarmBot leader
              facexy target_X target_Y
            ]
          ]
        ]
      ]
end 

to following-scent
  ask swarmRobots with [status = "following_scent"]
  [
    ifelse target != nobody
    [
      ;; either move to target or start disarming the mine
      ifelse distance target > 1
      [
        ifelse any? mines in-cone sensor-range sensor-FoV ;; [in-cone range fov_angle]
        [
          if mines != target
          [
            ifelse obs-detection = true
            [
              obs-avoidance
            ]
            [ ;; Simple obsticle avoidance
              right random 60
              left random 60
              fd .1
            ]

          ]
        ]
        [
          face target ;; important prevents zombie swarmbots
          fd .1
        ]
      ]
      [
        set status "disarming"
        set start-time ticks
        set color red
      ]
    ]
    [
      set status "foraging"
      set color blue
    ]
  ]
end 

to homming
  ;; when a bot finds a mine its at the distance near its sensor range
  ;; need to walk the bot up to the mine and then set bot status to disarming
  ;; need timeout
  ask swarmRobots with [status = "waiting"]
  [
    if target != nobody
    [
      ;; either move to target or start disarming the mine
      ifelse distance target > 1
      [
        face target ;; important prevents zombie swarmbots
        fd .1
        ask baseRobots
        [
          set color green
          set status "ON"
        ]
      ]
      [
        ; start disarming the mine
        set status "disarming"
        set start-time ticks
        set color red
      ]
    ]

  ]
end 

to disarming
  ;; check around the mine and make sure that there are the number of
  ;; required bots then set mine status to disarm and reset bots
  ;; and set bot status to foraging
  ask mines with [status = "found"]
  [
    ;; if the number of bots around the mine is corrent then return the bots to
    ;; foraging and destry the mine
    if (count swarmRobots in-radius 1) >= swarm-teamSize [
      ask swarmRobots in-radius 1 ;; check for bots in a radius around the mine
      [set status "foraging"
       set color blue
      ]
      set status "disarmed"
      ask mines with [status = "disarmed"] [die]
    ]
  ]
end 

to timeOut ;; this is a super important part of the system of systems... no timeout = zombies
  ;; resets a swarmBot that does not get help to a foraging bot
  ;; and idetntifies the mine
  ;; ***need to send this data to the baseRobot as a list of mines
  if mines != nobody[

    ask swarmRobots with [status = "disarming"]
    [
      if target != nobody
      [
        if (ticks - start-time) > Disarm_TimeOut
        [
          right 180
          fd random 10
          set status "foraging"
          set start-time ticks
          set color blue
          ask target [
            set status "hidden"
            set start-time ticks
            set color yellow
          ]

        ]
      ]
    ]

    ask mines with [status = "found"]
    [
      if mines != nobody
      [
        if (ticks - start-time) > Found_TimeOut [
          set status "hidden"
          set start-time ticks
          set size 1
          set color white
        ]
      ]
    ]

    ask swarmRobots with [status = "following_scent"]
    [
      if target != nobody
      [
        if (ticks - start-time) > Follow_Scent_TimeOut [
          set status "foraging"
          set start-time ticks
          set color blue
        ]

      ]
    ]

    ask swarmRobots with [status = "waiting"]
    [
      if target != nobody
      [
        if (ticks - start-time) > Follow_Scent_TimeOut [
          set status "foraging"
          set start-time ticks
          set color blue
        ]

      ]
    ]

  ]
end 

to blowup
  ;; this is fun - if my obsticle avoidance algo does not work well then the system will blow up.
  ask swarmRobots [
    if min-one-of other mines in-radius trigger-radius [distance myself]!= nobody
     [
      if min-one-of mines in-radius trigger-radius [distance myself] != target
        [
          if mines != nobody or swarmRobots != nobody [
            explode
            ask mines in-radius blast-radius [die]
            ask swarmRobots in-radius blast-radius [die]
          ]
        ]
     ]
  ]

  ask baseRobots [
    if min-one-of other mines in-radius trigger-radius [distance myself] != nobody
    [
      if mines != nobody or swarmRobots != nobody [
        explode
        ask baseRobots in-radius blast-radius [die]
        ask mines in-radius blast-radius [die]
        ask swarmRobots in-radius blast-radius [die]
        ]
    ]
  ]
end 


; This is where the explosion is created.
; EXPLODE calls hatch a number of times indicated by the slider FRAGMENTS.

to explode ; turtle procedure
  hatch-frags 10 [
    set dim 0
    rt random 360
    set size blast-radius
    set x-vel (x-vel * .5 + dx + (random-float 2.0) - 1)
    set y-vel (y-vel * .3 + dy + (random-float 2.0) - 1)
    pen-down
  ]
end 

; This function changes the color of a frag.
; Each frag fades its color by an amount proportional to FADE-AMOUNT.

to fade ; frag procedure
  ;set col one-of base-colors
  ;set color (col + 2)
  set dim dim - (10 / 10)
  set color scale-color color dim 5 .5
  ;show col
  if (color < (col - 3.0)) [ die ]
end 

to obs-avoidance
  ;; more complex obstical avoidance but I need to look at this
  ifelse min-one-of mines in-radius 1 [distance myself]!= nobody or min-one-of obstacles in-radius 1 [distance myself]!= nobody
   [face patch-right-and-ahead 90 1 fd 1 repeat 2 [face patch-left-and-ahead 90 1   fd 1] face patch-right-and-ahead 90 1 fd 1]
   [pd fd 1]
end 

to move_up
  ask baseRobots [set heading 0 fd 1]
end 

to move_rt
  ask baseRobots [set heading 90 fd 1]
end 

to move_lt
  ask baseRobots [set heading 270 fd 1]
end 

to move_dn
  ask baseRobots [set heading 180 fd 1]
end 


;; Includes from above

to make-msgQ [#id]
  set #msg-id #id
  set #msgQ []
end 

to-report msg-waiting?
  report (not empty? #msgQ)
end 

to-report get-msg
  let #tmp (first #msgQ)
  set #msgQ (but-first #msgQ)
  report #tmp
end 

to send-msg [#who-to #msg]
  let #who-from self
  ask turtles with [#msg-id = #who-to]
  [ set #msgQ (lput (list #who-from #msg) #msgQ)
  ]
end 

to broadcast [#breed-to #msg]
  let #who-from self
  set #msg (list #who-from #msg)
  ask turtles with [breed = #breed-to]
  [ set #msgQ (lput #msg #msgQ)
  ]
end 

to flush-msgQ [#id]
  set #msgQ []
end 

to flush-all-msgQs
  ask turtles
  [ set #msgQ []
  ]
end 

;; sxl-utils

to wiggle
  right 45 - (random 90)
end 

to-report nearest-of [#breed]
  report min-one-of #breed [distance myself]
end 

to-report nearest-of-xy [#breed #corx #cory]
  report min-one-of #breed [distancexy #corx #cory]
end 

to-report trigger [#prob]
  report (random 1000) < (#prob * 10)
end 

to-report mutate [#val #delta]
  let #mutator (random (#delta * 2 + 1)) - #delta
  report (#val + #mutator)
end 

to-report smooth-value [#last #inc #alpha]
  ;
  ; used in plotting to smooth graph data
  ; #alpha is the smoothing value 0-1
  ; larger #alphas are smoother, 0: no smoothing, 1: no recent value
  ;
  set #alpha (bounds 0 #alpha 1)
  let #new (#alpha * #last + (1 - #alpha) * #inc)
  report #new
end 

to-report bounds [#lo #x #hi]
  ;
  ; return #x bounded by #lo & #hi
  ;
  report (min (list #hi (max (list #lo #x))))
end