\ RANDOM-RUNNER#4 \ 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. If the turret is not aligned after the \ ranging, the robot repeats the alignment attempts described above. If the \ turret does not stay aligned after three aligment/ranging activities, the \ process loop is aborted. \ If alignment is maintained after ranging, the 70 ranges are normalized and \ then the best direction (hard-left, left, forward, right, or hard-right) is \ determined. If left or right is chosen, then the robot is turned in that \ direction for one-half second (about 45 degrees). If hard-left or hard-right \ is chosen, then the robot is turned in that direction for one second (about \ 90 degrees) If a best direction of "forward" is chosen, then the turning \ operation is skipped. \ After the optional turning operation is performed, 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 or until one of the rear bumper sensors are activated. If a rear bump \ sensor is activated, the robot stops immediately. If collisions occurs after \ three consecutive "forward" best directions, then the robot drives in reverse \ for four seconds or until a rear bump sensor is activated. If a rear bump \ sensor is activated, the robot stops immediately. After this reverse travel \ ends the robot is turn left for four seconds (about 180 degrees). Once the \ driving is completed the main loop is repeated until the lifespan \ time is passed. \ Note that the best direction code was changed to provide five, rather than \ three direction options. The drive code was changed to force a reversal of \ direction if the robot becomes trapped going forward multiple times. 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 2 (hard left) 1 (slight left), 0 (forward), \ -1 (slight right), or -2 (hard right). The code is written so the forward \ direction is favored, given that all direction range sums are equal. : BEST-DIRECTION ( -- dir ) 0 \ Initialize sum for the left side #2 17 0E DO I RANGES @ + \ Sum up nine ranges on the left side #2 LOOP 0 \ Initialize sum for the left side #1 0E 05 DO I RANGES @ + \ Sum up nine ranges on the left side #1 LOOP 0 \ Initialize sum for the front 05 0 DO I RANGES @ + \ Sum up five ranges on the left half of front LOOP 46 42 DO I RANGES @ + \ Sum up four ranges on the right half of front LOOP DUP \ Duplicate the front range sum 0 \ Initialize sum for the right side #1 42 39 DO I RANGES @ + \ Sum up nine ranges on the right side #1 LOOP SWAP \ Bring duplicate of the front range sum to TOS 0 \ Initialize sum for the right side #2 39 30 DO I RANGES @ + \ Sum up nine ranges on the right side #2 LOOP SWAP \ Bring duplicate of the front range sum to TOS 0 \ Place direction code of 0 onto TOS 3 FFFE DO \ Loop for index -2 to 2 -ROT \ Place direction value behind comparion values OVER \ Copy the new comparison value to TOS OVER \ Copy the current maximum value to TOS > IF \ New comparison value exceeds current maximum? DROP \ Drop the old maximum SWAP \ Bring old direction value to TOS DROP \ Drop the old direction value I \ Current index value is new direction code ELSE SWAP \ Bring failing comparison value to TOS DROP \ Discard the failing comparison value SWAP \ Bring direction value to TOS THEN LOOP SWAP \ Bring maximum value to TOS DROP \ Discard the maximum value ; \ STEER-ROBOT takes a direction value on the stack and spins the robot for one \ second. The flag value determines the direction of the spin. A positive \ direction (1, 2, 3, ...) causes the robot to spin to the left. A negative \ direction (-1 -2, -3, ...) causes the robot to spin to the right. After the \ robot spins for the chosen time period, 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 ( dir -- ) 64 \ Put value of 100 onto TOS SWAP \ Bring direction value to TOS DUP \ Duplicate the direction value 0> IF \ A positive value means turn to the left SPIN-LEFT \ Rotate robot left ELSE \ A "false" flag means that left is blocked more SPIN-RIGHT \ Rotate robot right THEN ABS \ Find absolute value of direction * \ Multiply ticks by absolute value of direction S>D WAIT-N-TICKS \ Run motors ABS(dir)/2 secs (100*ABS(dir) ticks) STOP \ Stop the motion C8 S>D WAIT-N-TICKS \ Allow motors 1 second (200 ticks) to settle ; \ Variable to count number of consecutive bumps while going forward VARIABLE BUMP_COUNT \ 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. \ of "0" indicates successful execution of the function. : 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 BUMP_COUNT DUP \ Load and duplicate the bump variable address @ 1+ SWAP ! \ Increment the count and save it 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= NOT IF \ A tick value of zero means a collision occurred 0 BUMP_COUNT ! \ Zero out the bump count as run was successful EXIT \ Leave the routine THEN REVERSE \ Drive reverse BUMP_COUNT @ DUP \ Load and duplicate the bump count 3 < IF \ Three collisions occur for three straight runs? 190 \ Run motor for 2 seconds (400 ticks) ELSE 320 \ Run motor for 4 seconds (800 ticks) THEN S>D \ Convert to a 32-bit number 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-REAR-RIGHT? \ A collision on rear right has occurred? BUMP-REAR-LEFT? \ A collision on rear 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 2DROP \ Discard the timeout value STOP \ Stop the motor C8 S>D WAIT-N-TICKS \ Allow motor 1 second (200 ticks) to settle 3 < IF \ Three collisions occur for three straight runs? EXIT \ Leave the routine THEN 4 STEER-ROBOT \ Turn robot ccw ~180 degrees 0 BUMP_COUNT ! \ Zero the bump count ; \ PROCESS-LOOP#4 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#4 ( lifespan -- code ) 0 BUMP_COUNT ! \ Clear the bump count BEGIN \ Loop for lifespan of operations TURRET-MOTOR-ON \ Turn on the turret motor 3 \ Push range repeat number onto the stack TURRET-ON-INDEX? \ Initial determation of turret alignment BEGIN \ Loop to determine if it is necessary to turn SWAP \ Bring range repeat number to TOS DUP \ Duplicate the range repeat number 0= IF DROP \ Discard the range repeat number DROP \ Discard the turret-on-index flag 2DROP \ Discard lifespan value FFF0 \ Push exit code onto TOS TURRET-MOTOR-OFF \ Turn off the turret motor EXIT \ Exit the process loop with error value at TOS ELSE 1- \ Decrement range repeat number by 1 SWAP \ Bring turret-on-index flag to TOS THEN NOT IF \ Determine if turret is not aligned 3 TURRET-ALIGN-ATTEMPTS \ Perform turret alignment DUP \ Duplicate result to preserve after test 0<> IF SWAP \ Move range repeat number to TOS DROP \ Discard range repeat number -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 64 S>D WAIT-N-TICKS \ Allow motor 1/2 second (100 ticks) to settle TURRET-ON-INDEX? \ Determine if turret is still aligned to index DUP \ Duplicate this flag for next loop if not aligned UNTIL \ Turret is still aligned after ranging DROP \ Discard extra turret-on-index flag DROP \ Discard range repeat number TURRET-MOTOR-OFF \ Turn off the turret motor NORMALIZE-RANGES \ Normalize the range values BEST-DIRECTION \ Determine best direction to travel DUP 0<> \ Determine if a turn right or left is chosen IF STEER-ROBOT \ Change direction according to easiest route 0 BUMP_COUNT ! \ Zero the bump count ELSE DROP \ Discard spare direction indicator THEN 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#4 is the fourth 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#4 ( 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#4 \ Enter processing loop ELSE -ROT 2DROP \ Discard lifespan value THEN RTC-HALT \ Halt the real time clock and exit ;