;turning corners

menu 1 [navigate]

menu 2 [go-straight]

menu 3 [setwhite l-ground]

menu 4 [align-with-white]

global [turn-count kill-turn white right-white left-white in-room right-wall-val left-wall-val front-val done]




;NAVIGATE

;This is the starting procedure. It tells
;Theseus what direction he should go when
;he starts out in the maze. The preference
;goes: left if there's no wall to the left,
;right if there's no wall to the right, and
;forward if both walls are present.

;After Theseus has chosen which start is appropriate,
;this procedure launches all of the appropriate 
;threads necessary for navigating the maze. It
;includes code for turning corners, maintaining
;its centered position in hallways, and distinguishing
;rooms. It also initializes the whiteness globals.
;
to navigate
  setdone 0
  setwhite 12
  setright-white 200
  setleft-white 200
  setin-room 0
  setkill-turn 1
  go-straight
  forever [drive]
  forever [watch-for-white]
  ;waituntil [done = 1] stoprules
end

;Specifies what mode Theseus should be in--navigate 
;the maze mode or inner room mode. 
;
;Allows Theseus to follow 
;the left walls of the maze. Bases his decision in whether 
;he's missing a left wall or not. 
;
;Watches the front of the robot and also the right.
;If there's a wall to the front and no wall to the 
;right, he turns right.

to drive
  ifelse in-room
    [track-room-wall]
    [
    if miss-left
	[find-wall-left
	go-straight wait 5]
    if (sees-front) 
        [dime-turn
         go-straight wait 5]
    if (and (between-walls) (not centered))
      [self-center]
    if wall-at-left 
      [straighten]
    ]
end

;Watches the floor for white marks. If Theseus has 
;already encountered white, he goes into find-the-circle
;mode and lines himself up with it. If he hasn't 
;encountered which before, he decides he's entered a
;room and, for the moment, does nothing.
;
to watch-for-white
  if done [stop]
  if close-front
    [backup
     wait 1
     go-straight]
  if (or over-white-l over-white-r)
    [
    ifelse in-room
      [ifelse kill-turn
        [;align-with-white
         fwd-motor off
         turn-motor off
         stoprules
         note 88 2 wait 1 note 88 1 wait 1 note 88 1 wait 1 note 93 3
         setdone 1
         ]
        [;stoprules ;new
         ;align-with-white ;new
         setin-room 0 
         ;go-straight ;new
         wait 30
	 ;forever [drive] ;new
	]
       ]
      [;stoprules ;new
       ;align-with-white ;new
       setin-room 1 
       ;go-straight ;new
       launch [note 88 2 wait 1 note 85 3]
       wait 30
       ;forever [drive] ;new
      ]
    ]
end




;TURNING

;Kills the straighten code and sets Theseus into
;turn mode. When he rediscovers the left wall, he
;kills the turn mode and re-enters straighten mode.
;
to find-wall-left
  setkill-turn 0
  	fwd-motor thatway setpower 6 on
	turn-motor thatway setpower 3 
  	launch [turn-bits]
	setleft-wall-val left-wall
  	waituntil [or (left-wall-val > 40) (over-white-l)]
  	setkill-turn 1
  go-straight
  wait 5
end

to find-wall-left-exodus
  setkill-turn 0
  	fwd-motor thatway setpower 6 on
	turn-motor thatway setpower 3 
  	launch [turn-bits]
	setleft-wall-val left-wall
        waituntil [not in-room]
  	;waituntil [or (turn-count > 5) (left-wall-val > 40)]
        ;ifelse (turn-count > 5)
        ;[go-straight
        ; waituntil [sees-front]
        ;]
	;waituntil [left-aligned]
  	setkill-turn 1
  go-straight
  wait 3
end

;Does the same thing as find-wall-left but for turning
;right.
;
to find-wall-right
  setkill-turn 0
  	fwd-motor thatway setpower 6 on
	turn-motor thisway setpower 3 
  	launch [turn-bits]
        setright-wall-val right-wall
  	waituntil [right-wall-val > 40] 
  	setkill-turn 1
  go-straight
end

;Allows the smooth turns for turn-right- and 
;turn-left-corner by turning the turn motor
;on intermittently. It kills the turn thread if
;the kill-turn variable is true.
;
to turn-bits
  setturn-count 1
  loop [
    if ((turn-count \\ 3) = 0 )
      [wait 1]
    if (turn-count > 7) 
	[go-straight
	 waituntil [sees-front]
         dime-turn]
    if close-front [backup wait 1]
    turn-motor onfor 1 wait 1
    setright-wall-val right-wall
    setleft-wall-val left-wall
    if (kill-turn) [stoprules stop]
    setturn-count (turn-count + 1)
  ]
end




;STRAIGHTENING

;Allows Theseus to keeep himself centered in the hallways.
;Uses a range of difference between (that is, which way it's 
;tilted) the two left-facing sensors. 
;
to straighten
  if (backleft-wall - left-wall) > 7 [add-left wait 1]
  if (left-wall - backleft-wall) > 7 [add-right wait 1]
end


to self-center
  if left-wall > right-wall [add-right wait 1]
  if right-wall > left-wall [add-left wait 1]
end




;INSIDE ROOM

;When inside a room, tracks the left wall and turns right.
;
to track-room-wall
  loop[
   if done [stop]
   if wall-at-left-room [straighten]
   if sees-front [dime-turn]
   if miss-left [wait 1 find-wall-left-exodus stop]
  ]
end

to dime-turn
  fwd-motor off
  turn-motor setpower 3 thisway on wait 2
  waituntil [left-aligned]
  go-straight
end

;Tells Theseus how to align with the white circle
;he encounters. If he encounters the circle with both
;sensors at once, he stops. Otherwise, he turns in a 
;fashion that will center him on the circle using
;backups and turns.
;
to align-with-white
  setleft-white l-ground
  setright-white r-ground
  loop [
    ;if (sees-front)
	;[backup
         ;wait 1
         ;go-straight
	 ;]
    ifelse (and (between left-white (white - 11) (white + 20)) 
	        (between right-white (white - 11) (white + 20)))
      [stop]
      [if over-white-l
         [setleft-white l-ground
          ;backup	  
          ;go-straight
	  turn-left
          waituntil [or (not over-white-l) (over-white-r)]
          ]
       if over-white-r
     	 [setright-white r-ground
	  ;backup
	  ;go-straight
	  turn-right
          waituntil [or (not over-white-r) (over-white-l)]
          ]
       ]
  ]  
end




;SENSOR CHECKS

;Is there a wall to the left?
;
to miss-left
  output (left-wall < 35)
end

;Is there a wall to the right?
;
to miss-right
  output (right-wall < 35)
end

;Is the robot near a left wall?
;
to wall-at-left
  output and (left-wall > 50) (backleft-wall > 50)
end

to wall-at-left-room
  output and (left-wall > 35) (backleft-wall > 35)
end

to between-walls
  output and (right-wall > 35) (left-wall > 35)
end

;Is the robot centered in the hallway?
;
to centered
  output between (right-wall - left-wall) (0 - 30) 30
end

to between :num :min :max
  output and (:num > :min) (:num < :max)
end

;Is the robot aligned with the left wall?
;
to left-aligned
  output between (backleft-wall - left-wall) (0 - 7) 7
end

;used in watch-front to tell if Theseus
;is approaching a wall in front of him. 
;
to sees-front
  output (front > 70)
end
  	
;Checks a given sensor for wall values.
;
to check-sensors :sensor
  ifelse (:sensor < 38)
  	[output 1] ; at doorway
  	[output 0] ; at wall
end

;outputs whether the left reflectance sensor
;senses white within a range of the white global
;variable.
;
to over-white-l
  output between l-ground (white - 11) (white + 15)
end

;Same as the left sensor, but for the right and with
;a bigger range since the sensor is more sensitive.
;
to over-white-r
  output between r-ground (white - 11) (white + 15)
end


;Watches the front of the robot to make sure it
;doesn't get too close to the front wall. If it does,
;it backtracks from the wall a bit and then continues
;on its merry way.
;
to close-front
  output (front > 100)
end




;DIRECTIONS

to turn-left
  fwd-motor off
  wait 2
  turn-motor setpower 2 on thatway
end

to turn-right
  fwd-motor off
  wait 2
  turn-motor setpower 2 on thisway
end

to backup
  turn-motor off wait 1
  fwd-motor thisway onfor 5
end

to go-straight
  turn-motor off
  fwd-motor setpower 7 on thatway
end

to add-left
  turn-motor setpower 1 thatway onfor 1
end

to add-right
  turn-motor setpower 1 thisway onfor 1
end




;SENSORS AND MOTORS

to left-wall
  output sensor 0
end

to right-wall
  output sensor 1
end

to backleft-wall
  output sensor 2
end

to front
  output sensor 5
end

to l-ground
  output sensor 4
end

to r-ground
  output sensor 3
end

to fwd-motor
  a,
  ;thisway is backward, thatway is forward
end

to turn-motor
  d,
  ;thisway is right, thatway is left
end