Small test program

This small test program drives in a straight line to the nearest obstacle, stopping 20cm in front of it and reports the distance driven.

VALUE Start_distance

: toWall

readSensors

OBSTACLE_FRONT to Start_distance

200 to leftPWM

150 to rightPWM

." Starting drive." cr

leftPWM rightPWM forward drive

BEGIN

readsensors

.state

OBSTACLE_FRONT 200 <

UNTIL

motor_stop

." End of drive: " cr

Start_distance Obstacle_front - .

." mm driven" cr

;