GoGo Mapper

;; There are two main problems that the robot must solve
;; 1) Figure out the layout of the room -- that is, what patches are solid, what aren't
;; 2) Calibrate itself the sensor range and turn speed, as well as allow for imprecise movements
;; This algorithm attempts to solve both problems at once.
;; It works by having many potential robots, each with somewhat characteristics and position.
;; Each potential robot checks it's accuracy against the sensor data. The confidence in each
;; potential robot is adjusted accordingly.
;; Then, each robot basically votes on which patches are solid and which are vacant, based on the
;; sensor data and the potential robot's attributes.
;; The best potential robots are then selected and the rest are removed.
;; The best robots reproduce with small variations.
;; The real robot and potential robots then move in an ideally similar fashion and the cycle repeats.
;;
;; Hopefully the robots conception of it's environment and itself converge.
;; The nice part of this algorithm is that it makes no assumptions about the sensitivity of the robot's
;; sensors, power of its motors, or the environment. It also requires no deduction, complex math,
;; or anything.
;;
;; It is loosely based on the particle filter algorithm used to orient robots in a known environment
;; while allowing for error in movements. This algorithm doesn't use any actual probability theory though.

extensions [ gogo ]

globals [
  serial-port
  test?
  controlable-srange
  ;; If the sensor reads > max-dist, we say it didn't hit anything
  time-step       ;; how long actions are done for
  max-confidence  ;; A patch can't have solidity > max-confidence; a robot can have confidence > max-confidence, but it only helps in selection, not reproduction
  previous-hit?
]

breed [ robots robot ]
breed [ fauxgos fauxgo ]  ;; fake gogo board robot for testing
breed [ echoes echo ]

robots-own [
  sensor-range ;; The sensors range in units of the distance the robot travels in time-step
  confidence    ;; Represents how confident we are in the bot
  turn-speed  ;; degrees the robot thinks it moves at a time
  sensor-position ;; how far in front of the center of rotation the sensor is
]

fauxgos-own [
  sensor-range
  turn-speed
  sensor-position
]

echoes-own [
  sensor-range
  sensor-position
  parent
  hit-patch
]

patches-own [
  solidity      ;; Represents how likely it is that the patch is solid
  solid?        ;; used for testing
]

to set-auton
  set autonomous-mode? true
end 

to set-auton-off
  set autonomous-mode? false
end 

to setup
  if not gogo:open? [ setup-gogo ]
  setup-brain
  set test? false
end 

to setup-gogo
  ifelse length (gogo:ports) > 0
    [ set serial-port user-one-of "Select a port:" gogo:ports ]
    [ user-message "There is a problem with the connection. Check if the board is on, and if the cable is connected. Otherwise, try to quit NetLogo, power cycle the GoGo Board, and open NetLogo again. For more information on how to fix connection issues, refer to the NetLogo documentation or the info tab of this model"
      stop ]
  gogo:open serial-port
  repeat 5
    [ if not gogo:ping
      [ user-message "There is a problem with the connection. Check if the board is on, and if the cable is connected. Otherwise, try to quit NetLogo, power cycle the GoGo Board, and open NetLogo again. For more information on how to fix connection issues, refer to the NetLogo documentation or the info tab of this model"] ]
  gogo:talk-to-output-ports [ "a" "b" "c" "d"]
end 

to setup-test
  setup-brain
  create-fauxgos 1 [
    set heading 0
    set turn-speed 3
    set sensor-position 3
    set shape "turtle"
    set size 5
    set sensor-range 3
  ]
  ask patches [
    set solid? pxcor = min-pxcor or
               pycor = min-pycor or
               pxcor = max-pxcor or
               pycor = max-pycor or
               (pxcor = 25 and pycor > -1) or
               (pxcor > -1 and pycor = 25) or
               (pxcor < 0 and pycor < 0 and (25 > abs (pxcor * pxcor + pycor * pycor - 625) ))
  ]
  ask patches with [solid?] [set pcolor white]
  set test? true
end 

to setup-brain
  ca
  set time-step 0.05
  set max-confidence 1000
  set previous-hit? false
  create-robots 1 [
    set heading 0
    set size 5
    set confidence max-confidence
    set turn-speed init-turn-speed
    set sensor-range init-sensor-range
    set sensor-position init-sensor-position
  ]
end 

to go
  sensor-check
  select-robots
  ask robots [reproduce]
  ask robots [set confidence 0]
  if autonomous-mode? = true
  [autonomous-move-behavior]
end 

to autonomous-move-behavior
  ifelse sense-dist > max-dist [robots-fd set previous-hit? false] [robots-rt robots-bk set previous-hit? true ]
end 

to autonomous-move-behavior1
  ifelse previous-hit? = false
  [ifelse sense-dist > max-dist [robots-fd set previous-hit? false] [robots-rt robots-bk set previous-hit? true ]]
  [ifelse sense-dist > max-dist [repeat 10 [robots-bk robots-lt sensor-check] set previous-hit? false] [robots-rt robots-bk set previous-hit? true ]]   
end 
;; observer procedure
;; Adjusts robots' confidences and patches' solidities based on sensor reading.

to sensor-check
  ask robots [set confidence confidence - 5 * [solidity] of patch-here]
  ask robots [fire-echo]
  ask robots [retract-echo]
  ask patches [
    if solidity < 0 [set solidity 0]
    if solidity > max-confidence [set solidity max-confidence]
    patch-recolor
  ]
  cd
end 

to-report sense-dist
  ifelse test? [
    let dist 0
    ask fauxgos [
      hatch-echoes 1 [
        set parent myself
        fd sensor-position
        while [distance myself < (sensor-range + sensor-position) and not [solid?] of patch-here and can-move? echo-speed] [
          fd echo-speed
        ]
        set dist distance myself - sensor-position
        die
      ]
    ]
    report 1.1 * max-dist * (random-normal 0 .1 + dist) / [sensor-range] of one-of fauxgos
  ] [report gogo:sensor 1]
end 

;; robot procedure
;; Gets the sensor's output in patches based on what this robot think's the sensor's range is

to-report get-dist
  report ifelse-value learn-sensor-range? [sensor-range][init-sensor-range] * sense-dist / max-dist
end 

;; robot procedure
;; Shoots an echo to the spot where the sensor sensed something (according to the robot)
;; The robots confidence level is adjusted on the way:
;; - The echo passing over a patch that the robot thinks is solid penalizes the robot
;; - The echo passing over a patch that the robot thinks is vacant rewards the robot slightly
;; - At the end, if the robot correctly predicted whether or not an obstacle was there, the robot is rewarded
;; - At the end, if the robot incorrectly predicted, the robot is penalized
;; All penalties/rewards are adjusted based on the solidity of the patches (as solidity sort of corresponds to confidence)

to fire-echo
  let dist get-dist
  hatch-echoes 1 [
    set size 1
    fd sensor-position
    pd
    set parent myself
  ]
  ask echoes with [parent = myself] [
    while [distance myself < (dist + sensor-position) and can-move? echo-speed] [
      let cur-patch patch-here
      ask parent [set confidence confidence + vacant-patch-reward-factor * (5 - [solidity] of cur-patch) / 10]
      fd echo-speed
    ]
    ifelse dist < sensor-range [
      set hit-patch patch-here
      ask parent [set confidence confidence + obstacle-reward-factor * ([solidity] of [hit-patch] of myself - (max-confidence / 3)) / 10]
    ] [
      set hit-patch nobody
    ]
  ]
end 

;; robot procedure
;; Like fire-echo, but adjusts solidity of the patches as the echo travels back to the robot.
;; The robots are essentially voting on what patches they think are solid

to retract-echo
  ask echoes with [parent = myself] [
    if hit-patch != nobody [ask hit-patch [ set solidity solidity + 5 ]]
    while [distance parent >= sensor-position] [
      face parent
      fd echo-speed
      if patch-here != hit-patch [ ask patch-here [ set solidity solidity - 1] ]
    ]
    die
  ]
end 

;; robot procedure
;; Each echo travels the same number of steps, otherwise it's biased to bigger sensor-ranges

to-report echo-speed
  report [sensor-range] of parent / 20
end 

;; observer procedure
;; Natural selection - best keep-robots robots are kept, rest die

to select-robots
  if count robots > keep-robots [
    ask min-n-of (count robots - keep-robots) robots [confidence] [die]
  ]
end 

;; robot procedure
;; Robots reproduce based on their confidence

to reproduce
  hatch-robots reproduce-amount [
    rt random-normal 0 .1
    if learn-sensor-range? [
      set sensor-range sensor-range + random-normal 0 sensor-range-mut-rate
    ]
    set color 5 + 10 * random 14
    if learn-turn-speed? [
      set turn-speed turn-speed + random-normal 0 turn-speed-mut-rate
    ]
    if learn-sensor-position? [
      set sensor-position sensor-position + random-normal 0 sensor-position-mut-rate
    ]
  ]
end 

to robots-fd
  ask robots [fd random-normal 1 vary-move]
  ifelse test? [
    ask fauxgos [
      let dist random-normal 1 0.1
      fd dist
      if [solid?] of patch-here [bk dist]
    ]
  ] [
    gogo:talk-to-output-ports ["a" "b"]
    gogo:set-output-port-power 7
    gogo:output-port-on
    wait time-step
    gogo:output-port-off
  ]
end 

to robots-bk
  ask robots [bk random-normal 1 vary-move]
  ifelse test? [
    ask fauxgos [
      let dist random-normal 1 0.1
      bk dist
      if [solid?] of patch-here [fd dist]
    ]
  ] [
    gogo:talk-to-output-ports ["a" "b"]
    gogo:set-output-port-power 7
    gogo:output-port-reverse
    gogo:output-port-on
    wait time-step
    gogo:output-port-off
    gogo:output-port-reverse
  ]
end 

to robots-rt
  ask robots [
    fd random-normal 0 0.01
    rt random-normal (ifelse-value learn-turn-speed? [turn-speed] [init-turn-speed]) vary-turn
  ]
  gogo-rt
end 

to gogo-rt
  ifelse test? [
    ask fauxgos [rt turn-speed]
  ] [
    gogo:talk-to-output-ports ["a" "b"]
    gogo:set-output-port-power 7
    gogo:output-port-on
    gogo:talk-to-output-ports ["a"]
    gogo:output-port-reverse
    wait time-step
    gogo:talk-to-output-ports ["b"]
    gogo:output-port-off
    gogo:talk-to-output-ports ["a"]
    gogo:output-port-off
    gogo:output-port-reverse
  ]
end 

to robots-lt
  ask robots [
    fd random-normal 0 0.01
    lt random-normal (ifelse-value learn-turn-speed? [turn-speed] [init-turn-speed]) vary-turn
  ]
  gogo-lt
end 

to gogo-lt
  ifelse test? [
    ask fauxgos [lt turn-speed]
  ][
    gogo:talk-to-output-ports ["a" "b"]
    gogo:set-output-port-power 7
    gogo:output-port-on
    gogo:talk-to-output-ports ["b"]
    gogo:output-port-reverse
    wait time-step
    gogo:talk-to-output-ports ["a"]
    gogo:output-port-off
    gogo:talk-to-output-ports ["b"]
    gogo:output-port-off
    gogo:output-port-reverse
  ]
end 

to patch-recolor
  set pcolor scale-color red solidity 0 max-confidence
end 

to robot-recolor
  set color scale-color color confidence 0 max-confidence
end