global [fdist fldist bldist frdist brdist
fwall rdoor ldoor
too-close too-far
interrupt decision
rev-thold turn-revs fwd-revs]

to check-calibration
loop [;white-display (bsr $142)
;red-display ((bsr $143) - 9)
white-display (bsr $145) ; front
;white-display (bsr $146) ; front right
]
end

to main
init
run
both-wheels off
room
both-wheels off
candle-align
both-wheels off
end

to run
hallway
both-wheels off
check-room
if (ir = 1) [stop]
exit-room
send 17 ;to Ckt 3 - done exiting
waituntil [newir?] ;waiting to see which way to turn
if (ir = 12)
[turn-left-corner]
if (ir = 13)
[turn-right-corner]
run
end

to room ;there is a candle in the room, so taking instructions from Cricket 2
waituntil [newir?]
if (ir = 3)
[both-wheels off
stop]
if (ir = 4)
[left-wheel off
right-wheel thisway on]
if (ir = 5)
[left-wheel thisway on
right-wheel off]
if (ir = 6)
[both-wheels thisway on]
if (ir = 7)
[both-wheels off]
end

to candle-align ;still taking instructions from Cricket 2
loop [waituntil [newir?]
if (ir = 14)
[left-wheel thisway on
right-wheel thatway on]
if (ir = 15)
[right-wheel thisway on
left-wheel thatway on]
if (if = 16)
; [go-forward ? ;we did not have time to determine how much farther
; stop] ;the robot would need to go forward at this point
end

to check-room
;go-forward
waituntil [(ir = 0) or (ir = 1)]
end

to exit-room ;candle is not in room
both-wheels thatway on
send 11
waituntil [ir = 7]
repeat 6
[waituntil [sensora > 200]
waituntil [sensora < 200]]
both-wheels off
stop
end

to init ; set variables before starting
setfwall 0
setrdoor 0
setldoor 0
settoo-close 0
settoo-far 0
setinterrupt 0
setdecision 0
setrev-thold 200
setturn-revs 11
setfwd-revs 24
end

to run
red-display 0 ;the LED red-display displays the number
setinterrupt 0 ;associated with the current procedure --
setdecision 0 ;this was one of our ways of debugging
both-wheels off
get-sensors
if (not (interrupt))
[follow-wall
both-wheels brake
setinterrupt 0
both-wheels off]
decide
if (decision = 0)
[corner-forward fwd-revs 0]
if (decision = 1)
[send 9 ;; tell Crick3 it's turning left
turn-left-corner
corner-forward fwd-revs 0]
if (decision = 2)
[send 8 ;; tell Crick3 it's turning right
turn-right-corner
corner-forward fwd-revs 1]
if (decision = 3)
[send 8 ;; tell Crick3 it's turning right
align
turn-right-corner
ifelse (fwall) ;; it hasn't rechecked sensors
[corner-forward fwd-revs 1]
[corner-forward fwd-revs 0]]
run
end

to follow-wall
red-display 1
get-sensors
if (interrupt)
[stop]
align
get-sensors
if (interrupt) [stop]
if (too-far)
[jog-left
ifelse (interrupt)
[stop]
[turn-right-align]]
if (too-close)
[jog-right
ifelse (interrupt)
[stop]
[turn-left-align]]
settoo-far 0
settoo-close 0
both-wheels thisway on
follow-wall
end

to go-forward :distance
red-display 16
both-wheels thisway on
repeat :n
[waituntil [sensora > rev-thold]
waituntil [sensora < rev-thold]]
both-wheels brake
end

;; procedures to keep robot parallel to wall

to align
red-display 2
both-wheels off ;brake doesn't work here
resett
loop [if (timer > 1000)
[go-forward 4
beep stop]
get-lside
if (((difference fldist bldist) > 50)
and (fldist < 100))
[setinterrupt 1 stop]
ifelse (less-than bldist fldist)
[tweak-right] ;we had to put the tweak methods in-line
[ifelse (less-than fldist bldist) ;due to stack overflow, but we have left them
[tweak-left] ;as abstracted procedures here for clarity
[stop]
]
]
end

to tweak-right
red-display 3
right-wheel thatway on
left-wheel thisway on
delay 50
both-wheels off
if ((difference fldist bldist) > 50)
[if ((fldist < 100) or (bldist < 100))
[tweak-left
setinterrupt 1]]
end

to tweak-left
red-display 4
left-wheel thatway on
right-wheel thisway on
delay 40
both-wheels off
if ((difference fldist bldist) > 50)
[if ((fldist < 100) or (bldist < 100))
[tweak-right
setinterrupt 1]]
end

;; procedures to keep robot in middle of hall

to jog-right
red-display 5
get-lside
left-wheel thisway on
right-wheel thatway on
wait 2
both-wheels thisway on
repeat 2
[get-sensors
if (interrupt)
[stop]]
end

to jog-left
red-display 6
get-lside
right-wheel thisway on
left-wheel thatway on
wait 2
both-wheels thisway on
repeat 2
[get-sensors
if (interrupt)
[stop]]
end

to turn-right-align
red-display 7
right-wheel off
left-wheel thisway on
loop [get-lside
if (not (less-than bldist fldist))
[both-wheels off stop]]
end

to turn-left-align
red-display 8
left-wheel off
right-wheel thisway on
loop [get-lside
if (not (less-than fldist bldist))
[both-wheels off stop]]
end

;; procedures to turn a corner

to decide
red-display 9
get-sensors-special
if ((difference fldist bldist) > 50)
[prepare-decide]
ifelse (fwall)
[ifelse (rdoor and ldoor)
[setdecision ((random % 2) + 1)]
[if (rdoor)
[setdecision 3]
if (ldoor)
[setdecision 1]
]
]
[ifelse (rdoor)
[setdecision ((random % 2) * 3)]
[ifelse (ldoor)
[setdecision (random % 2)]
[setdecision 0]
]
]
if (fdist < 110)
[go-forward 2]
end

to prepare-decide
both-wheels thisway on
ifelse (rdoor)
[loop [get-sensors-special
if (fwall) [stop]
if (brdist < 120) [stop]
]
]
[if (ldoor)
[loop [get-sensors-special
if (fwall) [stop]
if (bldist < 100) [stop]
]
]
]
end

to get-sensors-special
setfdist (bsr $145)
setbldist ((bsr $143) - 9)
setfldist (bsr $142)
setfrdist (bsr $146)
setbrdist (bsr $147)
setfwall (fdist > 100)
setrdoor (frdist < 120)
setldoor (fldist < 100)
end

to turn-left-corner
red-display 10
right-wheel thisway on
left-wheel thatway on
repeat turn-revs
[waituntil [sensora > rev-thold]
waituntil [sensora < rev-thold]]
both-wheels brake
end

to turn-right-corner
red-display 11
left-wheel thisway on
right-wheel thatway on
repeat turn-revs
[waituntil [sensora > rev-thold]
waituntil [sensora < rev-thold]]
both-wheels brake ;; had off
end

to go-forward :distance :lwall
red-display 12
repeat 4
[both-wheels thisway on
repeat (:distance / 4)
[waituntil [sensora > rev-thold]
waituntil [sensora < rev-thold]]
both-wheels brake
if (newir?) [stop]
if (:lwall) [align]]
end

;; procedures to test environment

to get-sensors
;;red-display 13
setfdist (bsr $145)
ifelse (fdist > 110)
[setinterrupt 1]
[setfldist (bsr $142)
setfrdist (bsr $146)
ifelse ((fldist < 100) and (frdist < 120))
[setinterrupt 1]
[setbldist ((bsr $143) - 9)
setbrdist (bsr $147)
if ((fldist > 180) and (bldist > 180))
[settoo-close 1]
if ((fldist < 160) and (bldist < 160))
[settoo-far 1]
]
]
end

to get-lside
;;red-display 14
setfldist (bsr $142)
setbldist ((bsr $143) - 9)
end

to get-rside
;;red-display 15
setfrdist (bsr $146)
setbrdist (bsr $147)
end

to delay :n
repeat :n [setfwall fwall]
end

;; wheel procedures

to both-wheels
ab,
end

to right-wheel
b,
end

to left-wheel
a,
end

;; math procedures

to abs :n
ifelse :n < 0
[output ( -1 * :n)]
[output :n]
end

to equals :x :y
ifelse (abs (:x - :y) < 5)
[output 1]
[output 0]
end

to less-than :x :y
ifelse (:x < (:y - 5))
[output 1]
[output 0]
end

to difference :x :y
output (abs (:x - :y))
end