LEGO MINDSTORMS Robots

Page 139
DECIMAL
42 CONSTANT TURNAROUND_TIME
10 CONSTANT NUMBER_OF_SAMPLES
VARIABLE threshold
VARIABLE returnTime
: sensorValue
BEGIN DUP SENSOR_READ 0= UNTIL
SENSOR_VALUE
;
: calibrate
0
NUMBER_OF_SAMPLES 0 DO
2 sensorValue +
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
armGrab
BEGIN 2 sensorValue 100 = UNTIL
armRelease
BEGIN 2 sensorValue 100 < UNTIL
armStop
;
: release
armRelease
BEGIN 2 sensorValue 100 = UNTIL
armGrab
BEGIN 2 sensorValue 100 < UNTIL
armStop
;
: turnAround
spin
TURNAROUND_TIME sleep
stop
;