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

Скачать файл
Заказать решение

ВУЗ: Не указан

Категория: Не указан

Дисциплина: Не указана

Добавлен: 12.06.2025

Просмотров: 1932

Скачиваний: 0

ВНИМАНИЕ! Если данный файл нарушает Ваши авторские права, то обязательно сообщите нам.

Page 98 · Advanced Robotics with the Toddler

MxCurrent

VAR

Word

Sx

VAR

Word

SxCurrent

VAR

Word

' -----

[ EEPROM

Data ]

-----------------------------------------------------

' ------

Movement Support ------

Codes

' The following

state tables are lists of movement state numbers.

' A xx indicates the end of a list.

' These are used with the

Movement routine.

TL

CON

0

TC

CON

1

TR

CON

2

SL

CON

3

SC

CON

4

SR

CON

5

xx

CON

255

'------ Movement Value Tables ------

'These can be used with the Movement routine.

'The tables can contain Basic Movement Codes.

'Note: ALL movement tables must be in this section

LeftSemicircle

DATA

7, bLeftTurn,

bLeftTurn, bForward,

xx

RightSemicircle

DATA

7, bRightTurn,

bRightTurn, bForward,

xx

WalkForward3

DATA

3,

bForward,

xx

WalkForward8

DATA

8,

bForward,

xx

'------ Basic Movement Codes ------

'' Used in Movement tables.

'Referenced below using LOOKUP statement.

bFinish

CON

0

bForward

CON

1

bBackward

CON

2

bLeftTurn

CON

3

bRightTurn

CON

4

bPivotLeft

CON

5

bPivotRight

CON

6

' ------ Basic Movement

Tables ------

'' These tables

can contain Movement Support Codes.

BasicMovements

CON

Forward

Forward

DATA

1, TR, SL, TL, SR, xx

Chapter #5: Following Light · Page 99

Backward

DATA

1, TR, SR, TL, SL, xx

LeftTurn

DATA

1, TL, SR, TC, SL, xx

RightTurn

DATA

1, TR, SL, TC, SR, xx

PivotLeft

DATA

3, TL, SR, TC, SL, xx

PivotRight

DATA

3, TR, SL, TC, SR, xx

Finish

DATA

1, TR, SC, TC, xx

'

-----[ Initialization ]--------------------------------------------------

GOSUB ResetCC

'

Initialize feet

'

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

Main:

' Measure RC time for left photoresistor.

HIGH LPhotoCircuit

'

Set to output-high.

PAUSE 3

'

Pause for 3 ms.

RCTIME LPhotoCircuit,1,LPhotoVal

'

Measure R/C time on left

' Measure RC time for right photoresistor.

HIGH RPhotoCircuit

'

Set to output-high.

PAUSE 3

'

Pause for 3 ms

RCTIME RPhotoCircuit,1,RPhotoVal

'

Measure R/C time on right

' Measure difference between RPhotoVal and LPhotoVal, decide what to do DEBUG home, "Left = ", dec LPhotoVal, " Right = ",dec RPhotoVal,cr

IF ABS(LPhotoVal-RPhotoVal) < DeadBand THEN main

IF LPhotoVal > RPhotoVal THEN turn_right

IF LPhotoVal < RPhotoVal THEN turn_left

'-----

Navigation Routines -------

Turn_left:

' turn left towards light

Mx =

PivotLeft

GOSUB Movement

GOTO

main

' go back to main routine.

Turn_right:

' turn right towards light

Mx =

PivotRight

GOSUB Movement

GOTO

main

' go back to main routine.

' -----

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

' -----

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

'

Page 100 · Advanced Robotics with the Toddler

'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

MxCurrent = Mx

' setup to use submovement

MoveLoopLimit = 1

' table

GOTO StartMovement

SetupMovement:

READ Mx, MoveLoopLimit

' read movement table

MxCurrent = Mx + 1

' repeat count

StartMovement:

FOR MoveLoop = 1 to MoveLoopLimit

Mx = MxCurrent

' Mx = start of movement

' table

'DEBUG DEC Mx, " Movement ", DEC MoveLoop, " of ", DEC MoveLoopLimit,CR

IF Mx < BasicMovements THEN MovementLoop

' skip

if

movement table

SxCurrent = Mx

' SxCurrent = submovement

table index

GOTO

StartSubMovement

'

enter middle of loop

MovementLoop:

READ

Mx, SxCurrent

'

read

next submovment byte

Mx = Mx + 1

IF SxCurrent = xx THEN MovementDone

' skip if end of list 'DEBUG " ", DEC SxCurrent, " movement",CR

LOOKUP SxCurrent,[Finish,Forward,Backward,LeftTurn, RightTurn,PivotLeft,PivotRight],SxCurrent

' lookup submovement table index

StartSubMovement:

' execute submovement table

READ SxCurrent, SubMoveLoopLmt

' read submovement table

SxCurrent = SxCurrent + 1

FOR SubMoveLoop = 1 TO SubMoveLoopLmt

Sx = SxCurrent

'DEBUG "

", DEC Sx, " submovement "

'DEBUG DEC SubMoveLoop, " of ", DEC SubMoveLoopLmt,CR

SubMovementLoop:

READ Sx, Dx

' read next submovement

Sx = Sx + 1

' action

Chapter #5: Following Light · Page 101

IF Dx

= xx THEN SubMovementDone

'

skip if

end of list

GOSUB

DoMovement

'

execute

movement

GOTO SubMovementLoop

SubMovementDone:

NEXT

IF Mx < BasicMovements THEN MovementLoop

MovementDone:

NEXT

RETURN

DoMovement:

'DEBUG " ", DEC Dx, " action", CR

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

StrideCenter,StrideRight]

' will fall through if invalid index

RETURN

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

TiltLeft:

NewValue = LeftTilt

GOTO MovementTilt

TiltCenter:

NewValue = CenterTilt

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

Page 102 · Advanced Robotics with the Toddler

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

How the Light Compass Works

Program 5.2 takes RC time measurements and first checks to see if the difference between the values returned by the RCTIME commands fall in the DeadBand variable using the command:

IF ABS(LPhotoVal-RPhotoVal) < DeadBand THEN main

If the difference between RC times is within the DeadBand, the program jumps to the Main: label. If the measured difference in RC times is not within the DeadBand, two IF...THEN statements decide which routine to call, Turn_left or Turn_right.

IF ABS(LPhotoVal-RPhotoVal) < DeadBand THEN main

IF LPhotoVal > RPhotoVal THEN turn_right

IF LPhotoVal < RPhotoVal THEN turn_left

Смотрите также файлы