\ RANDOM-RUNNER#2 \ This is a high-level set of routines that implement an automaton the travels \ in random directions based on feedback from the turret ranger and the front \ bump sensors. This code requires the RTXEB patches (rtxeb_patches.fs), real \ time clock functions (rtc.fs), sensor functions (sensors.fs), drive functions \ (drive.fs), turret control functions (turret.fs), and ranger control \ functions (ranger.fs) to be loaded first. \ Once the real-time clock is initialized, the robot tries to align the turret \ three times; aborting the function if alignment fails. If alignment \ succeeds, the process loop is entered. The process loop first tests for \ turret alignment. If not aligned, the robot tries to align the turret three \ times; aborting the function if alignment fails. If alignment succeeds, then \ a 360-degree ranging is performed. The 70 ranges are normalized and then the \ best direction (left, forward, or right) is determined. If left or right is \ chosen, then the robot is turned in that direction for one second (about 90 \ degrees). The loop is repeated until a best direction of "forward" is \ chosen. \ When the best direction is "forward", the robot drives forward for eight \ seconds or until one of the front bump sensors are activated. If a front \ bump sensor is activated, then the robot drives in reverse for one second. \ Once the driving is completed the main loop is repeated until the lifespan \ time is passed. \ Note that the turret realignment code has been moved into the inner loop. \ This was done in order to bring the turret back into alignment after every \ turret ranging activity and turn instead of after every forward run. HEX \ All numerical values are in hexidecimal format \ TURRET-ALIGN-ATTEMPTS calls the function ALIGN-TURRET up to "count" number of \ times or until the turret alignment is successful. The final alignment error \ code is left on the stack. For a success alignment, the value is zero. : TURRET-ALIGN-ATTEMPTS ( count -- code ) DUP \ Duplicate the count value to pad the stack 0 DO \ Enter do-loop for turret alignment attempts DROP \ Discard result of previous alignment attempt ALIGN-TURRET \ Perform turret alignment DUP \ Duplicate result to preserve after test 0= IF \ Is turret now aligned? LEAVE \ Exit the loop; code on stack will be zero THEN LOOP \ Loop until count done; failure code at TOS ; \ End of ALIGN-ATTEMPTS \ BEST-DIRECTION uses the ranging data created by TURRET-RANGER and normalized \ by NORMALIZE-RANGES (see ranger.fs) to determine the best direction that \ should be taken by the robot. In this version, the possible values returned \ on the stack (called "dir") are 1 (left), 0 (forward), or -1 (right). : BEST-DIRECTION ( -- dir ) 0 \ Initialize sum for the front 05 0 DO I RANGES @ + \ Sum up five ranges on the left half of front LOOP 46 41 DO I RANGES @ + \ Sum up five ranges on the right half of front LOOP 0A / \ Divide sum by ten to renormalize 0 \ Initialize sum for the right side 41 37 DO I RANGES @ + \ Sum up ten ranges on the right side LOOP 0A / \ Divide sum by ten to renormalize 0 \ Initialize sum for the left side 0F 05 DO I RANGES @ + \ Sum up ten ranges on the left side LOOP 0A / \ Divide sum by ten to renormalize OVER OVER \ Make copies of left and right ranges >= IF \ Is right range greater or equal to left range? DROP \ True, so discard the left range >= IF \ Is front range greater or equal to right range? 0 \ True, "0" means continue forward ELSE -1 \ False, "-1" means turn right (cw) THEN ELSE SWAP DROP \ False, so discard the right range >= IF \ Is front range greater or equal to left range? 0 \ True, "0" means continue forward ELSE 1 \ False, "1" means turn left (ccw) THEN THEN ; \ STEER-ROBOT takes a flag value on the stack and spins the robot for one \ second. The flag value determines the direction of the spin. A true (-1) \ flag causes the robot to spin to the left. A false (0) flag causes the \ robot to spin to the right. After the robot spins for one second, the \ motors are turned off and a delay of one second is taken to allow the motors \ to come to a full stop. : STEER-ROBOT ( flag -- ) IF \ A "true" flag means that right is blocked more SPIN-LEFT \ Rotate robot left ELSE \ A "false" flag means that left is blocked more SPIN-RIGHT \ Rotate robot right THEN C8 S>D WAIT-N-TICKS \ Run motors for 1 seconds (200 ticks) STOP \ Stop the motion C8 S>D WAIT-N-TICKS \ Allow motors 1 second (200 ticks) to settle ; \ DRIVE-ROBOT starts the robot moving forward for a maximum travel time of \ eight seconds. During the travel, the front bumper switches are monitored for \ a collision. If no collision occurs, the robot is stopped for one second. \ If a collision occurs, the robot is stopped for one second, then driven in \ reverse for one second, and then stopped again for one second. : DRIVE-ROBOT ( -- ) FORWARD \ Drive forward 640 S>D \ Run motor for 8 seconds (1600 ticks) TICK_VAL 2@ \ Load the 32-bit ticker D+ \ Increment the value by the number of ticks BEGIN \ Begin loop for driving and monitoring bumps WAIT-FOR-NEXT-TICK \ Wait for 0.005 seconds BUMP-FRONT-RIGHT? \ A collision on front right has occurred? BUMP-FRONT-LEFT? \ A collision on front left has occurred? OR IF \ Either or both occuring will stop the robot 2DROP 0 S>D \ Replace ending tick value with zero THEN 2DUP \ Duplicate the ending tick value TICK-N-PASSED? \ Determine if the ticker has passed this "time" UNTIL \ Loop until timeout or collision occurs STOP \ Stop the motion C8 S>D WAIT-N-TICKS \ Allow motor 1 second (200 ticks) to settle D0= IF \ A tick value of zero means a collision occurred REVERSE \ Drive reverse 190 S>D WAIT-N-TICKS \ Allow motor 2 seconds (400 ticks) to settle STOP \ Stop the motion C8 S>D WAIT-N-TICKS \ Allow motor 1 second (200 ticks) to settle THEN ; \ PROCESS-LOOP is the main execution function of the RANDOM-RUNNER automaton. \ The TOS and NEXT contain the double number value "lifespan", which is the \ number of RTC clock ticks that the automaton will run. After execution, the \ TOS contains the value "code", which indicates the completion code. A value \ of "0" indicates successful execution of the function. : PROCESS_LOOP ( lifespan -- code ) BEGIN \ Loop for lifespan of operations BEGIN \ Loop to determine if it is necessary to turn TURRET-MOTOR-ON \ Turn on the turret motor TURRET-ON-INDEX? \ Determine if turret is already aligned NOT IF 3 TURRET-ALIGN-ATTEMPTS \ Perform turret alignment DUP \ Duplicate result to preserve after test 0<> IF -ROT \ Move alignment failure code behind lifespan 2DROP \ Discard lifespan value TURRET-MOTOR-OFF \ Turn off the turret motor EXIT \ Exit the process loop with error value at TOS THEN DROP \ Discard exit code THEN RANGER-INIT \ Perform turret ranger initialization TURRET-RANGER \ Perform 360-degree range RANGER-HALT \ Stop turret ranger configuration TURRET-MOTOR-OFF \ Turn off the turret motor C8 S>D WAIT-N-TICKS \ Allow motor 1 second (200 ticks) to settle NORMALIZE-RANGES \ Normalize the range values BEST-DIRECTION \ Determine best direction to travel DUP 0<> \ Determine if a turn right or left is chosen WHILE \ Forward direction is obstructed 0> STEER-ROBOT \ Change direction according to easiest route REPEAT \ Repeat while front of robot is obstructed DROP \ Discard direction indicator DRIVE-ROBOT \ Drive robot straight a distance 2DUP TICK-N-PASSED? \ Determine if lifespan has been passed UNTIL \ Loop until lifespan is passed STOP \ Make sure that the drive motors are halted 2DROP 0 \ Return code of "0" means success ; \ RANDOM-RUNNER#2 is the second in a series of basic automatons that cause the \ robot to travel around in random directions according to interactions with \ its environment. There are no inherent goals other than to avoid collisions. \ The TOS and NEXT contain the double number value "lifespan", which is the \ number of RTC clock ticks that the automaton will run. After execution, the \ TOS contains the value "code", which indicates the completion code. A value \ of "0" indicates successful execution of the function. : RANDOM-RUNNER#2 ( lifespan -- code ) RTC-INIT \ Initialize the real time clock TURRET-MOTOR-ON \ Turn on the turret motor 3 TURRET-ALIGN-ATTEMPTS \ Try three times to align the turret TURRET-MOTOR-OFF \ Turn off the turret motor, saving motor phase DUP \ Duplicate result to preserve after test 0= IF \ Determine if initial alignment was successful DROP \ Discard test result to place lifespan at TOS PROCESS_LOOP \ Enter processing loop ELSE -ROT 2DROP \ Discard lifespan value THEN RTC-HALT \ Halt the real time clock and exit ;