Файл: Advanced Robotics with the Toddler (Paralax, student guide, v1.3, 2004).pdf

'The command "gosub check_sensors" sends the program to a subroutine

'that loads distance values into l_values and r_values. So, when the

'fprogram returns rom the check_sensors subroutine, the values are

'updated and ready for distance based decisions.

GOSUB check_sensors

'The distances are checked for four different inequalities. Depending

'on the inequality that turns out to be true, the program either

'branches to the forward, left_turn, right_turn or backward navigation

'routine. The "3" value used below to test the boundary conditions

'may need to be changed depending upon the color of the walking surface

'and the angle of IR LEDs and detectors.

'-----[ Subroutines ]-----------------------------------------------------

'The check sensors subroutine is a modified version of Program Listing

'6.1 without the debug Terminal display. Instead of displaying l_values

'and r_values, the main routine uses these values to decide which way to

'go.

check_sensors:

'Load sensor outputs into l_values and r_values using a FOR..NEXT loop

'a lookup table, and bit addressing.

FOR counter = 0 TO 4

check_left_sensors:

LOOKUP counter,[37500,38250,39500,40500,41500],l_IR_freq

Chapter #7: Staying on the Table · Page 145

FREQOUT lEmitter, 1, l_IR_freq l_values.lowbit(counter) = ~ lDetector

check_right_sensors:

LOOKUP counter,[37500,38250,39500,40500,41500],r_IR_freq FREQOUT rEmitter, 1, r_IR_freq

r_values.lowbit(counter) = ~ rDetector

NEXT

'Convert l_values and r_values from binary to ncd format.

l_values = ncd l_values r_values = ncd r_values

'Now l_values and r_values each store a number between 0 and 5

'corresponding to the zone the object is detected in. The program can

'now return to the part of the main routine that makes decisions based

'on these distance measurements.

RETURN

'----- Movement: Move feet using DATA table referenced by Mx -----

'Input: Mx = movement table index, table ends in xx

'or

'Mx = submovement table index, table ends in xx

'

' Note: All submovment tables come after the movment tables in this file.

Movement:

IF Mx < BasicMovements THEN SetupMovement

FOR SubMoveLoop = 1 TO SubMoveLoopLmt

Sx = SxCurrent

DEBUG " ", DEC Sx, " submovement ", DEC SubMoveLoop, " of " DEBUG DEC SubMoveLoopLmt,CR

SubMovementDone: NEXT

IF Mx < BasicMovements THEN MovementLoop

' exit if submovement table

MovementDone: NEXT

RETURN

DoMovement:

DEBUG " ", dec Dx, " action",cr

BRANCH Dx,[TiltLeft,TiltCenter,TiltRight,StrideLeft,StrideCenter,

StrideRight]

' will fall through if invalid

RETURN

' ---- Movement routines can be called directly ----

TiltLeft:

NewValue = LeftTilt

GOTO MovementTilt

TiltCenter:

NewValue = CenterTilt

Chapter #7: Staying on the Table · Page 147

GOTO MovementTilt

TiltRight:

NewValue = RightTilt

MovementTilt:

FOR Pulses = CurrentTilt TO NewValue STEP TiltStep

PULSOUT TiltServo, Pulses

PULSOUT StrideServo, CurrentStride

PAUSE MoveDelay

NEXT

CurrentTilt = NewValue

RETURN

StrideLeft:

NewValue = LeftStride

GOTO MovementStride

StrideCenter:

NewValue = CenterStride

GOTO MovementStride

StrideRight:

NewValue = RightStride

MovementStride:

FOR Pulses = CurrentStride TO NewValue STEP StrideStep

PULSOUT TiltServo, CurrentTilt

PULSOUT StrideServo, Pulses

PAUSE MoveDelay

NEXT

CurrentStride = NewValue

RETURN

' ----- Move feet to initial center position -----

ResetCC:

CurrentTilt = CenterTilt

CurrentStride = CenterStride

FOR Pulses = 1 TO 100 STEP StrideStep

PULSOUT TiltServo, CenterTilt

PULSOUT StrideServo, CenterStride

PAUSE MoveDelay

NEXT

DoReturn:

RETURN

Page 148 · Advanced Robotics with the Toddler

Aliased Variables

The Drop-off Detection program in Program 7.2 is the beginning of a rather large program in terms of data memory. In fact, without a little PBASIC programming trick, the program will not compile. The trick is PBASIC’s ability to alias a variable so it uses the storage space of another variable. This allows the program to run with the 16 words of RAM space (actually 3 words are used for the BASIC Stamp’s PBASIC and interface pin support).

The following code from Program Listing 7.2 shows how the aliasing is done.

The first VAR definition is normal. It defines a nibble variable. The next four reuse different variables. They are the same size as the aliased variables. The main requirement to keep in mind when using aliased variables is that any variables sharing the same storage that these variables cannot be used at the same time. In other words, do not try the following.

l_values = 1 Mx = 2

Aliasing is normally used because the original variable names do not work well with a new part of the program or subroutine. PBASIC has no concept of local variables so aliasing is required.

Chapter #7: Staying on the Table · Page 149

The BASIC Stamp’s IDE can present the memory map of the current program. This provides RAM and EEPROM usage information. The memory map for the Toddler Program 7.2 is shown in Figure 7.4. It shows 5 bytes of free RAM. Not much but enough. This includes the use of four word aliased variables. If these variables were not aliased then the program would need additional 8 bytes, 3 more than available.

Figure 7-4: EEPROM Memory Map for Toddler Program 7.2

Aliasing should be used with great care. It is a significant source of problems when debugging a program. The advantage of using this with the BASIC Stamp is that only a limited number of variables will be used in the program so it is readily apparent where problems occur.

In this case, the initial set of variables including Mx is used in the movement part of the program. Only the Mx variable is used outside of the Movement routine and that is used to pass a parameter to the routine. The aliased variables including l_IR_freq variable is used in the range finding routine. Since these two routines do not call each other it is easy to isolate the two with respect to variables.

Page 150 · Advanced Robotics with the Toddler

How the Drop-off Avoidance Program Works

Now that we have the aliasing issue out of the way we can move onto the main program. The first thing the main routine does is call the check_sensors subroutine. Note that check_sensors is simply Program 7.1 with no Debug Terminal display placed in a subroutine. Instead of debugging the NCD values of l_detect and r_detect, the values of these two variables are simply converted to NCD values using the statements:

l_values = ncd l_values

and

r_values = ncd r_values

After calling the check_sensors subroutine, l_values and r_values are numbers between “0” and “5.” After the program returns from the check_sensors subroutine, l_values and r_values are checked against the benchmarks distance indicating the edge of the table has been detected.

boundary CON 2

IF l_values >= boundary AND r_values >= boundary THEN go_forward IF l_values >= boundary AND r_values < boundary THEN left_turn IF l_values < boundary AND r_values >= boundary THEN right_turn IF l_values < boundary AND r_values < boundary THEN go_backward

The routines then load the Mx variable with the index of the appropriate table. The Movement routine then uses the table to initiate the Toddler’s leg movements. The boundary value is the distance boundary condition. This may need to be changed depending upon the color of the surface the Toddler is walking on. It must be set so that the Toddler reliably sees the table when moving forward.

The angle at which the IR LEDs and sensors can be tilted downward is limited so a low boundary value is typical. One alternative to having a value of 1 or 2 is to adjust the range finding frequencies so that the midrange values are sensing distances farther away. The other alternative is to mount the IR LEDs and sensors closer or on to the Toddler’s feet.

The current configuration with the IR LEDs and sensors mounted on the Toddler’s circuit board does lead to a long rang recognition of the edge of the table so the Toddler should not get much closer than a foot from the edge. This means the Toddler needs a relatively large table with a white or light colored surface to walk on.