Page 150 - The Unofficial Guide to Lego Mindstorms Robots

139

DECIMAL

42 CONSTANT TURNAROUND_TIME
10 CONSTANT NUMBER_OF_SAMPLES

VAR IABLE threshold
VARIABLE returnTime

: sensorValue
BEGIN DUP SENSOR_READ 0= UNTIL
SENSOR_VALUE
;

: calibrate
0
NUMBER_OF_SA MPLES 0 DO
2 se nsorValue +
1 sleep
LOOP
NUMBER_OF_SAMPLES /
threshold !
threshold @ showValue
;

: forward 7 1 2 MOTOR_ SET ;
: spin 7 2 2 MOTOR_SET ;
: stop 7 3 2 MOTOR_SET ;

: armGrab 7 1 0 MOTOR_SET ;
: armRelease 7 2 0 MOTOR_SET ;
: armStop 7 3 0 MOTOR_SET ;

: grab
ar mGrab
BEG IN 2 sensorValue 100 = UNTIL
armRelease
BEGIN 2 sensorValue 100 < UNTIL
armStop
;

: release
armRelease
BEGI N 2 sensorValue 100 = UNTIL
armGrab
BEGIN 2 sensorValue 100 < UNTIL
armStop
;

: turnAround
spin
TURNAROUND_ TIME sleep
stop
;