; changes
; Fri Aug 27 11:34:58 2004 fredm
; fixed bugs in first posting
;
; Fri Aug 20 18:34:44 2004 fredm
; added commands for 2nd board
; added sensor1, sensor2, ... sensor8
; added switch1, switch2, ... switch8
; jumper across ID holes for 2nd board
;
; sensors on first board are 1-4; 2nd board are 5-8
;
; motor commands c, cd, and d, are already built in to the Cricket!

global [x]

to sensor1
output sens 1
end

to sensor2
output sens 2
end

to sensor3
output sens 3
end

to sensor4
output sens 4
end

to sensor5
output sens 5
end

to sensor6
output sens 6
end

to sensor7 
output sens 7
end

to sensor8
output sens 8
end

to switch1
output (sens 1) < 128
end

to switch2
output (sens 2) < 128
end

to switch3
output (sens 3) < 128
end

to switch4
output (sens 4) < 128
end

to switch5
output (sens 5) < 128
end

to switch6
output (sens 6) < 128
end

to switch7
output (sens 7) < 128
end

to switch8
output (sens 8) < 128
end

to sens :n
bsend $112
ifelse :n < 5
  [setx bsr $1f + :n]
  [setx bsr $3b + :n]
output x * 256 + bsr 0
end

to counts :n
bsend $112
ifelse :n < 5
  [setx bsr $27 + :n]
  [setx bsr $43 + :n]
output x * 256 + bsr 0
end

to resetc :n
bsend $112
ifelse :n < 5
  [bsend $2f + :n]   ; counter 1 is at $10
  [bsend $4b + :n]
bsend 0
end

to velo :n
bsend $112
ifelse :n < 5
  [setx bsr :n + $23]
  [setx bsr :n + $3f]
output x * 256 + bsr 0
end

to quad12
bsend $112
setx bsr 12
output x * 256 + bsr 0
end

to resetq12
bsend $112 bsend $14 bsend 0
end

to quad34
bsend $112
setx bsr 13
output x * 256 + bsr 0
end

to resetq34
bsend $112 bsend $15 bsend 0
end

; if you jumper the ID holes, then
; motor c becomes e and motor d becomes f.
to e,
c, ; don't talk to a or b
bsend $13f ; talkto nobody
bsend $133 ; talkto e
end

to f,
c,
bsend $13f
bsend $134
end

to ef,
c,
bsend $13f
bsend $133 bsend $134
end