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

Page 174 · Advanced Robotics with the Toddler

MoveDelay

CON

17

' to make up 20 ms servo loop

CenterTilt

CON

750

CenterStride

CON

750

prepBumper

CON

%1001

bothLEDsOff

CON

%1001

'

-----[ Variables ]-------------------------------------------------------

Counter

VAR

Nib

Temp

VAR

Bit

'

-----[ Initialize ]------------------------------------------------------

ResetFeet:

FOR Counter = 1 TO 15

' center feet for .3 sec

GOSUB Move_It

' currentMove is only a nibble.

PAUSE MoveDelay

' make up 20 ms servo refresh time

NEXT

Test_Speaker:

FREQOUT Spkr, 2000, 3000

' program start/restart signal.

'

-----[ Main Code ]-------------------------------------------------------

Main_Program:

DO

FREQOUT LeftIRLED,1,38500

' pulse left IRLED.

IF (LeftIR_Sensor =

0) THEN

' check for IR hit on left

DEBUG "

IR left

"

ELSE

DEBUG "

"

ENDIF

FREQOUT RightIRLED,1,38500

' repeat for the right IR pair.

IF (RightIR_Sensor = 0) THEN

' check for IR hit on right

DEBUG "

IR Right

"

ELSE

DEBUG "

"

ENDIF

FREQOUT RightIRLED,1,38500

' repeat for the right IR pair.

Do_Bumpers:

DIRS = prepBumper

OUTS = bothLEDsOff

Chapter #8: Real-Time Programming with Bumpers and Infrared · Page 175

IF (LeftBumper = 0) THEN

' check for bumper hit on left

DEBUG " bump left

"

ELSE

DEBUG "

"

ENDIF

IF (RightBumper = 0) THEN

' check for bumper hit on right

DEBUG " bump Right ", CR

ELSE

DEBUG "

", CR

ENDIF

LOOP

END

'

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

Move_It:

PULSOUT TiltServo,

CenterTilt

PULSOUT StrideServo, CenterStride

RETURN

TiltServo

PIN

13

' tilt servo on X7

StrideServo

PIN

12

' stride servo on X6

LeftIRLED

PIN

4

' left IRLED

RightIRLED

PIN

15

' right IRLED

LeftIRSensor

PIN

11

' left IR sensor input

RightIRSensor

PIN

14

' right IR sensor input

RightBumper

PIN

1

LeftBumper

PIN

2

LeftVisionLED

PIN

5

' left vision display LED

RightVisionLED

PIN

6

' right vision display LED

Spkr

PIN

8

' beeper pin

' -----[ Constants ]-------------------------------------------------------

MoveDelay

CON

1

' code already using 19 ms of 20

TiltStep

CON

5

' TiltServo step size

StrideStep

CON

5

' StrideServo step size

RightTilt

CON

610

' tilt limits

CenterTilt

CON

750

LeftTilt

CON

880

RightStride

CON

625

' stride limits

CenterStride

CON

750

LeftStride

CON

875

ctrlLEDs

CON

%1111

' define DIRs as constants to

Chapter #8: Real-Time Programming with Bumpers and Infrared · Page 177

prepBumper

CON

%1001

' simplify LED/bumper setups

bothGreen

CON

%0110

' OUTa register sets red/green

bothRed

CON

%1001

' status on P0-P3 for Twinkle Toes

rightRed

CON

%0101

' object detection states

leftRed

CON

%1010

bothLEDsOff

CON

%1001

Fwd

CON

0

' codes to pick movement table

PivL

CON

1

' set newMove to one of these

PivR

CON

2

' values and New_Movemt will LOOKUP

BumpL

CON

3

' the right movement table

BumpR

CON

4

' -----[ Variables ]-------------------------------------------------------

doneTiltFlag

VAR

Bit

' flag: 1 = tilt reached new value

doneMoveFlag

VAR

Bit

' flag: 1 = servos hit new values

rightIR_Flag

VAR

Bit

' flag: 1 = something on right side

leftIR_Flag

VAR

Bit

' flag: 1 = something left side

rightBumpFlag

VAR

Bit

' flag: 1 = bumper hit right side

leftBumpFlag

VAR

Bit

' flag: 1 = bumper hit left side

sensors

VAR

Nib

' lower 2 bits of the sensors var

' used to store IR detector values

currentMove

VAR

Nib

newMove

VAR

Nib

Mx

VAR

Word

' index for movement tables

MxOffset

VAR

Byte

' added to Mx for index

bMovmnt

VAR

Byte

' table value for lookup movement

currentTilt

VAR

Word

currentStride

VAR

Word

newTilt

VAR

Word

newStride

VAR

Word

TL

CON

0

' use lower nibble for these

TC

CON

1

TR

CON

2

SL

CON

3

SC

CON

4

SR

CON

5

xx

CON

255

' table end code

' ------ Basic Movement

Tables ------

'