Shop OBEX P1 Docs P2 Docs Learn Events
Toddler Roaming problem — Parallax Forums

Toddler Roaming problem

CrazyrabbitCrazyrabbit Posts: 116
edited 2013-10-27 20:50 in Robotics
I recently got a used Toddler and so far so good. I got the book off Google and having some problems with some of the book programs. I have got the IR detectors working after finding some small mistakes on the IR setup programs in the book. The wrong pin for the left IR should have been 4 instead of 10. Now I am working on the roaming program. It has more problems. I tweaked it a bit so it would load. but doesn't to anything except the servo jerking on startup; As far as I can go without making a whole new program. Can the book program be fixed or do you have a more recently debugged program. It is more complicated then I thought. Here is a copy of the roaming program I am working on.


Also I don;t know how to insert a PBASIC or PROP progams.

' Toddler Program 6.2: Object Detection And Avoidance
' {$Stamp bs2} ' Stamp Directive.
'
[ I/O Definitions ]
TiltServo CON 13 ' Tilt servo on P12
StrideServo CON 12 ' Stride servo on P13
'
[ Constants ]
MoveDelay CON 25 ' in micrcoseconds
TiltStep CON 10 ' TiltServo step size
RightTilt CON 620 ' Tilt limits
CenterTilt CON 750
LeftTilt CON 880
StrideStep CON 10 ' StrideServo step size
RightStride CON 650 ' Stride limits
CenterStride CON 750
LeftStride CON 850
'
[ Variables ]
FigureLoop VAR Nib
MoveLoop VAR Byte ' Loop for repeat movements
MoveLoopLimit VAR Byte
SubMoveLoop VAR Byte ' Loop for repeat submovements
SubMoveLoopLimit VAR Byte
Pulses VAR Word ' Pulse variable
CurrentTilt VAR Word
CurrentStride VAR Word
NewValue VAR Word
Dx VAR Pulses
Mx VAR Word
MxCurrent VAR Word
Sx VAR Word
SxCurrent VAR Word
'
[ 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
Backward DATA 1, TR, SR, TL, SL, xx
LeftTurn DATA 1, TL, SR, TC, SL, TL, SR, TR, SL, xx
RightTurn DATA 1, TR, SL, TC, SR, TR, SL, TL, SR, xx
PivotLeft DATA 3, TL, SR, TC, SL, TR, SR, TC, SL, xx
PivotRight DATA 3, TR, SL, TC, SR, TL, SL, TC, SR, xx
Finish DATA 1, TR, SC, TC, xx
'
Declarations
' The lower 2 bits of the
sensors VAR Nib ' sensors variable are used to store
' IR detector values.
left_pin CON 4
right_pin CON 15
left_in VAR IN11
right_in VAR IN14
'
Initialization

OUTPUT 2 ' Set all I/O lines sending freqout
OUTPUT left_pin ' signals to function as outputs
OUTPUT right_pin
FREQOUT 2, 2000, 3000 ' Program start/restart signal.
GOSUB ResetCC ' Initialize feet
'
Main Routine
main:
FREQOUT left_pin,1,38500 ' Send freqout signal - left IRLED.
sensors.BIT0 = left_in ' Store IR detector output in RAM.
' Detect object on the right.
FREQOUT right_pin,1,38500 ' Repeat for the right IR pair.
PAUSE 18 ' 18 ms pause(2 ms lost on freqout).
' By loading the IR detector output values into the lower 2 bits of the sensors
' variable, a number btwn 0 and 3 that the branch command can use is generated.
LOOKUP sensors,[Backward,PivotLeft,PivotRight,Forward],Mx
GOSUB Movement
GOTO main
'
[ Subroutines ]
'
'
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
MxCurrent = Mx ' setup to use submovement table
MoveLoopLimit = 1
GOTO StartMovement
SetupMovement:
READ Mx, MoveLoopLimit ' read movement table repeat count
MxCurrent = Mx + 1
StartMovement:
FOR MoveLoop = 1 TO MoveLoopLimit
Mx = MxCurrent ' Mx = start of movement table
DEBUG HEX 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 " ", HEX SxCurrent, " movement",CR

LOOKUP SxCurrent,[Finish,Forward,Backward,LeftTurn,RightTurn,PivotLeft,PivotRight],SxCurrent
' lookup submovement table index
StartSubMovement: ' start executing submovement table
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 " ", HEX SxCurrent, " movement",CR

LOOKUP SxCurrent,[Finish,Forward,Backward,LeftTurn,RightTurn,PivotLeft,PivotRight],SxCurrent
' lookup submovement table index
'StartSubMovement: ' start executing submovement table
READ SxCurrent, SubMoveLoopLimit
' read submovement table repeat count
SxCurrent = SxCurrent + 1
FOR SubMoveLoop = 1 TO SubMoveLoopLimit
Sx = SxCurrent
DEBUG " ", HEX Sx, " submovement ", DEC SubMoveLoop, " of ", DEC SubMoveLoopLimit,CR
SubMovementLoop:
READ Sx, Dx ' read next submovent action
Sx = Sx + 1
IF Dx = xx THEN SubMovementDone
' skip if end of list
GOSUB DoMovement ' execute movement
GOTO SubMovementLoop
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 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
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

Comments

Sign In or Register to comment.