' {$STAMP BS1}
' {$PBASIC 1.0}

'------------------------------------------------------
' Dual Servo Random Movement Generator
' By Vern Graner April 28th, 2005
' Any questions to vern@graner.com
' R-04.28b
'------------------------------------------------------
' -First attempt for Prop-1 controller
'------------------------------------------------------
' Hardware setup:
' Servos connected to pins P0-1


SYMBOL RESULT = W0 ' value can be 0 to 65535
'SYMBOL      I = B0 ' value can be 0 to 255
'SYMBOL      X = B1 ' value can be 0 to 255
SYMBOL  DEST1 = B2 ' value can be 0 to 255
SYMBOL   CUR1 = B3 ' value can be 0 to 255
SYMBOL  DEST2 = B4 ' value can be 0 to 255
SYMBOL   CUR2 = B5 ' value can be 0 to 255

'SYMBOL  SERVO = B2 ' value can be 0 to 255


'DEBUG "go!"

'Pre set values
'********************************************
RESULT  = 11000     ' set initial "seed" value
CUR1    = 50
DEST1   = 50
CUR2    = 50
DEST2   = 50
'********************************************

LOOP:

IF CUR1=DEST1 THEN FetchNewDest1   'If the servo has reached it's destination fetch a new one
  GOTO NextTest
FetchNewDest1:
  RANDOM RESULT
  RESULT=RESULT // 200
  DEST1=RESULT
  DEST1=DEST1 MIN 50
  DEST1=DEST1 MAX 200
'  DEBUG "Cur1=",#CUR1," Dest1=",#DEST1,CR

NextTest:
IF CUR2=DEST2 THEN FetchNewDest2   'If the servo has reached it's destination fetch a new one
  GOTO RandomDone
FetchNewDest2:
  RANDOM RESULT
  RESULT=RESULT // 200
  DEST2=RESULT
  DEST2=DEST2 MIN 50
  DEST2=DEST2 MAX 200
'  DEBUG "Cur2=",#CUR2," Dest2=",#DEST2,CR

RandomDone:

IF CUR1<DEST1 THEN PlusCur1        'Check to see if CUR is less than DEST
CUR1=CUR1-1                        'If NOT the decrement CUR
'DEBUG "after dec cur1=",#CUR1,CR
GOTO TestDone1                     'Jump out of the test
PlusCur1:
CUR1=CUR1+1                        'If SO then increment CUR
'DEBUG "after inc cur1=",#CUR1,CR
TestDone1:

IF CUR2<DEST2 THEN PlusCur2        'Check to see if CUR is less than DEST
CUR2=CUR2-1                        'If NOT the decrement CUR
GOTO TestDone2                     'Jump out of the test
PlusCur2:
CUR2=CUR2+1                        'If SO then increment CUR
TestDone2:


'DEBUG CLS
'DEBUG "CUR1=",#CUR1," DEST1=",#DEST1,CR
'DEBUG "CUR2=",#CUR2," DEST2=",#DEST2,CR
PULSOUT 0,CUR1                    'send a pulse to the servo
PULSOUT 1,CUR2                    'send a pulse to the servo
PAUSE 20
GOTO LOOP